pablogs9
commented
7 months ago Could you provide the code you are using?
Closed Pansamic closed 7 months ago
pablogs9
commented
7 months ago Could you provide the code you are using?
Pansamic
commented
7 months ago Two files: 1.microros.c ; 2.freertos.c.
freertos.c calls microros_init() in microros.c to initialize micro-ros, microros_init() init rclc kernel, publishers and subscribers.
First part
/**
* @file microros.c
* @author pansamic (pansamic@foxmail.com)
* @brief micro-ROS related functions
* @version 0.1
* @date 2023-11-15
*
* @copyright Copyright (c) 2023
*
*/
/******************************************************/
/* INCLUDES */
/******************************************************/
#include <time.h>
#include <microros.h>
/* STM32 Peripherals */
#include "rtc.h"
#include "tim.h"
/* Board Peripheral drivers */
#include "motor.h"
#include "led.h"
#include "icm20602.h"
#include "mecanum.h"
/******************************************************/
/* DEFINES */
/******************************************************/
/******************************************************/
/* MACROS */
/******************************************************/
#define RCCHECK(fn) \
{\
rcl_ret_t temp_rc = fn;\
if (RCL_RET_OK != temp_rc) {\
printf("Failed status on line %d: %d. Aborting.\n", __LINE__, (int)temp_rc);\
return 1;\
}\
}
#define RCSOFTCHECK(fn) \
{\
rcl_ret_t temp_rc = fn;\
if(RCL_RET_OK != temp_rc) {\
printf("Failed status on line %d: %d. Continuing.\n", __LINE__, (int)temp_rc);\
}\
}
/******************************************************/
/* VARIABLES */
/******************************************************/
/*****************************/
/* micro ros core components */
/*****************************/
rclc_support_t support={0};
rcl_allocator_t allocator={0};
rcl_allocator_t freeRTOS_allocator={0};
rclc_executor_t executor={0};
rclc_parameter_server_t param_server={0};
/* micro ros parameter server config */
const rclc_parameter_options_t param_server_options =
{
.notify_changed_over_dds = true,
.max_params = 4,
.allow_undeclared_parameters = true,
.low_mem_mode = false
};
const char * paramname_wheel_dis_x = "wheel_dis_x";
const char * paramname_wheel_dis_y = "wheel_dis_y";
const char * paramname_wheel_radius = "wheel_radius";
const char * paramname_pulse_per_round = "pulse_per_round";
double paramval_wheel_dis_x = 0.193;
double paramval_wheel_dis_y = 0.223;
double paramval_wheel_radius = 0.075;
int paramval_pulse_per_round = 16800;
/*****************************/
/* micro ros communication */
/*****************************/
rcl_node_t node={0};
rcl_subscription_t sub_twist={0};
rcl_subscription_t sub_time_ref={0};
rcl_publisher_t pub_imu={0};
rcl_publisher_t pub_odometry={0};
rcl_publisher_t pub_tf2 = {0};
rcl_publisher_t pub_joint_states={0};
/*****************************/
/* micro ros messages */
/*****************************/
nav_msgs__msg__Odometry msg_odometry={0};
sensor_msgs__msg__Imu msg_imu={0};
geometry_msgs__msg__Twist msg_twist={0};
sensor_msgs__msg__TimeReference msg_time_ref={0};
tf2_msgs__msg__TFMessage msg_tf2={0};
geometry_msgs__msg__TransformStamped tf2_data={0};
sensor_msgs__msg__JointState msg_joint_state={0};
joint_states_t joint_states={0};
/******************************************************/
/* DECLARATION */
/******************************************************/
/*****************************/
/* port comm interface */
/*****************************/
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);
/*****************************/
/* memory management */
/*****************************/
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);
/* set wheel velocity subscriber callback function */
void set_wheel_vel_cb(const void * msgin);
/* set time reference subscriber call back function */
void set_time_ref_cb(const void * msgin);
/* parameter server callback function */
bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context);
extern uint16_t ReleaseTime;
/******************************************************/
/* DEFINITION */
/******************************************************/
void microros_init(void)
{
uint8_t ret=0;
ret |= microros_init_kernel();
ret |= microros_init_param_server();
ret |= microros_init_imu_publisher();
ret |= microros_init_odom_publisher();
// ret |= microros_init_tf2_publisher();
ret |= microros_init_joint_states_publisher();
ret |= microros_init_timeref_subscriber();
ret |= microros_init_twist_subscriber();
if(ret)
{
Set_RGBLED(RED_ON,GREEN_OFF,BLUE_OFF);
while(1){}
}
else
{
Set_RGBLED(RED_OFF,GREEN_ON,BLUE_OFF);
}
}
uint8_t microros_init_kernel(void)
{
rcl_ret_t ret;
/***************************************/
/* allocator init */
/***************************************/
rmw_uros_set_custom_transport(
true,
NULL,
cubemx_transport_open,
cubemx_transport_close,
cubemx_transport_write,
cubemx_transport_read);
// detect micro ros agent
printf("[INFO] waiting for micro ros agent.\r\n");
while(rmw_uros_ping_agent(100,1)!=RMW_RET_OK)
{
}
/* Here you also give to the allocator the functions
* that are going to be used in order to allocate memory etc.
*/
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]set default allocators failed!\r\n");
}
allocator = rcl_get_default_allocator();
/***************************************/
/* support init */
/***************************************/
// create init_options
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
ret = rcl_init_options_init(&init_options, allocator);
ret = rcl_init_options_set_domain_id(&init_options, 3); // set ROS_DOMAIN_ID=3
ret = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
if(ret == RCL_RET_OK)
{
printf("[INFO] init support OK!\r\n");
}
else
{
printf("[ERROR]init support failed!\r\n");
return 1;
}
/***************************************/
/* node init */
/***************************************/
// create node
ret = rclc_node_init_default(&node, "mecanum_microros_node", "", &support);
if(ret == RCL_RET_OK)
{
printf("[INFO] init node OK!\r\n");
}
else
{
printf("[ERROR]init node failed!\r\n");
return 1;
}
/***************************************/
/* executor init */
/***************************************/
executor = rclc_executor_get_zero_initialized_executor();
/* 2 handles, time_ref subscription and cmd_vel_mecanum subscription */
/* `RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES` are handles that param server needs */
ret = rclc_executor_init(&executor, &support.context, RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES+2, &allocator);
if(ret == RCL_RET_OK)
printf("[INFO] init executor OK\r\n");
else
{
printf("[ERROR]init executor failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_param_server(void)
{
rcl_ret_t ret;
// Initialize parameter server with default configuration
ret = rclc_parameter_server_init_with_option(¶m_server, &node, ¶m_server_options);
if (ret == RCL_RET_OK)
{
printf("[INFO] init param server OK!\r\n");
ret = rclc_add_parameter(¶m_server, paramname_wheel_dis_x, RCLC_PARAMETER_DOUBLE);
ret = rclc_parameter_set_double(¶m_server, paramname_wheel_dis_x, paramval_wheel_dis_x);
ret = rclc_add_parameter(¶m_server, paramname_wheel_dis_y, RCLC_PARAMETER_DOUBLE);
ret = rclc_parameter_set_double(¶m_server, paramname_wheel_dis_y, paramval_wheel_dis_y);
ret = rclc_add_parameter(¶m_server, paramname_wheel_radius, RCLC_PARAMETER_DOUBLE);
ret = rclc_parameter_set_double(¶m_server, paramname_wheel_radius, paramval_wheel_radius);
ret = rclc_add_parameter(¶m_server, paramname_pulse_per_round, RCLC_PARAMETER_INT);
ret = rclc_parameter_set_int(¶m_server, paramname_pulse_per_round, paramval_pulse_per_round);
// Add parameter server to the executor including defined callback
ret = rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed);
}
else
{
printf("[ERROR]init param server failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_imu_publisher(void)
{
rcl_ret_t ret;
// create publisher
ret = rclc_publisher_init_default(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"imu/data");
if (ret == RCL_RET_OK)
printf("[INFO] imu publisher init OK!\r\n");
else
{
printf("[ERROR]imu publisher init failed!\r\n");
return 1;
}
msg_imu.header.frame_id.data = "base_link";
msg_imu.header.frame_id.size = 9;
msg_imu.header.frame_id.capacity = 9;
for(uint8_t i=0 ; i<9 ; i++)
msg_imu.angular_velocity_covariance[i]=-1;
for(uint8_t i=0 ; i<9 ; i++)
msg_imu.linear_acceleration_covariance[i]=-1;
for(uint8_t i=0 ; i<9 ; i++)
msg_imu.orientation_covariance[i]=-1;
return 0;
}
uint8_t microros_init_odom_publisher(void)
{
rcl_ret_t ret;
ret = rclc_publisher_init_default(&pub_odometry,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),"/odom/unfiltered");
if (ret == RCL_RET_OK)
{
printf("[INFO] odometry publisher init OK!\r\n");
static char odometry_frame_id[] = "odom";
static char odometry_child_frame_id[] = "base_link";
msg_odometry.header.frame_id.data = odometry_frame_id;
msg_odometry.header.frame_id.capacity = 5;
msg_odometry.header.frame_id.size = 5;
msg_odometry.child_frame_id.data = odometry_child_frame_id;
msg_odometry.child_frame_id.capacity = 10;
msg_odometry.child_frame_id.size = 10;
}
else
{
printf("[ERROR]odometry publisher init failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_tf2_publisher(void)
{
rcl_ret_t ret;
ret = rclc_publisher_init_default(&pub_tf2,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage),"tf");
if(ret == RCL_RET_OK)
{
printf("[INFO] TF publisher init OK!\r\n");
msg_tf2.transforms.data = &tf2_data;
msg_tf2.transforms.data->header.frame_id.data = "odom";
msg_tf2.transforms.data->header.frame_id.capacity = 5;
msg_tf2.transforms.data->header.frame_id.size = 4;
msg_tf2.transforms.data->header.stamp.sec = 0;
msg_tf2.transforms.data->header.stamp.nanosec = 0;
msg_tf2.transforms.data->child_frame_id.data = "base_link";
msg_tf2.transforms.data->child_frame_id.capacity = 10;
msg_tf2.transforms.data->child_frame_id.size = 9;
msg_tf2.transforms.data->transform.translation.x = 0;
msg_tf2.transforms.data->transform.translation.y = 0;
msg_tf2.transforms.data->transform.translation.z = 0;
msg_tf2.transforms.data->transform.rotation.w = 1;
msg_tf2.transforms.data->transform.rotation.x = 0;
msg_tf2.transforms.data->transform.rotation.y = 0;
msg_tf2.transforms.data->transform.rotation.z = 0;
msg_tf2.transforms.capacity = 1;
msg_tf2.transforms.size = 1;
}
else
{
printf("[ERROR]TF publisher init failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_joint_states_publisher(void)
{
rcl_ret_t ret = rclc_publisher_init_default(&pub_joint_states,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),"joint_states");
if (ret == RCL_RET_OK)
{
printf("[INFO] joint_state publisher init OK!\r\n");
/* initialize joint_state message, modify data container 'joint_states',
* rather than the message variable. */
static char joint1_name_data[] = "left_front_wheel_joint";
static char joint2_name_data[] = "left_rear_wheel_joint";
static char joint3_name_data[] = "right_front_wheel_joint";
static char joint4_name_data[] = "right_rear_wheel_joint";
joint_states.joint_names[0].data = joint1_name_data;
joint_states.joint_names[0].capacity = 24;
joint_states.joint_names[0].size = 24;
joint_states.joint_names[1].data = joint2_name_data;
joint_states.joint_names[1].capacity = 23;
joint_states.joint_names[1].size = 23;
joint_states.joint_names[2].data = joint3_name_data;
joint_states.joint_names[2].capacity = 25;
joint_states.joint_names[2].size = 25;
joint_states.joint_names[3].data = joint4_name_data;
joint_states.joint_names[3].capacity = 26;
joint_states.joint_names[3].size = 26;
msg_joint_state.name.data = joint_states.joint_names;
msg_joint_state.name.capacity = 4;
msg_joint_state.name.size = 4;
msg_joint_state.position.data = joint_states.position;
msg_joint_state.position.capacity = 4;
msg_joint_state.position.size = 4;
msg_joint_state.velocity.data = joint_states.velocity;
msg_joint_state.velocity.capacity = 4;
msg_joint_state.velocity.size = 4;
msg_joint_state.effort.data = joint_states.effort;
msg_joint_state.effort.capacity = 4;
msg_joint_state.effort.size = 4;
}
else
{
printf("[ERROR]joint_state publisher init failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_timeref_subscriber(void)
{
rcl_ret_t ret;
// create a mecanum wheel message
sensor_msgs__msg__TimeReference__init(&msg_time_ref);
const rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs,msg,TimeReference);
// Initialize a reliable subscriber
ret = rclc_subscription_init_best_effort(&sub_time_ref, &node,type_support, "time_reference");
if (ret == RCL_RET_OK)
printf("[INFO] time_reference subscriber init OK!\r\n");
else
{
printf("[ERROR]time_reference subscriber init failed!\r\n");
return 1;
}
// Add subscription to the executor
ret = rclc_executor_add_subscription(&executor, &sub_time_ref, &msg_time_ref,&set_time_ref_cb, ON_NEW_DATA);
if (ret == RCL_RET_OK)
printf("[INFO] executor add subscription of /time_reference OK!\r\n");
else
{
printf("[ERROR]executor add subscription of /time_reference failed!\r\n");
return 1;
}
return 0;
}
uint8_t microros_init_twist_subscriber(void)
{
rcl_ret_t ret;
// create a mecanum wheel message
geometry_msgs__msg__Twist__init(&msg_twist);
// Initialize a reliable subscriber
ret = rclc_subscription_init_best_effort(&sub_twist, &node,ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist), "cmd_vel");
if (ret == RCL_RET_OK)
printf("[INFO] twist msg subscriber init OK!\r\n");
else
{
printf("[ERROR]twist msg subscriber init failed!\r\n");
return 1;
}
// Add subscription to the executor
ret = rclc_executor_add_subscription(&executor, &sub_twist, &msg_twist,&set_wheel_vel_cb, ON_NEW_DATA);
if (ret == RCL_RET_OK)
printf("[INFO] executor add subscription of /cmd_vel_mecanum OK!\r\n");
else
{
printf("[ERROR]executor add subscription of /cmd_vel_mecanum failed!\r\n");
}
return 0;
}
void microros_sync_time(void)
{
rmw_ret_t sync_ret = rmw_uros_sync_session(100);
if(sync_ret == RMW_RET_OK)
{
printf("[INFO] sync time OK!\r\n");
}
else
{
printf("[ERROR]sync time failed!\r\n");
}
}
void microros_spinsome(void)
{
rclc_executor_spin_some(&executor,100);
}
void set_wheel_vel_cb(const void * msgin)
{
geometry_msgs__msg__Twist * msg = (geometry_msgs__msg__Twist*) msgin;
if(msg == NULL)
{
printf("[ERROR]received NULL wheel vel msg!\r\n");
}
else
{
Car.TargetXVelocity = msg->linear.x;
Car.TargetYVelocity = msg->linear.y;
Car.TargetAngularVelocity = msg->angular.z;
ReleaseTime = ENCODER_UPDATE_INTERVAL * 10;
printf("[INFO] recv twist msg: linear X=%.3f | linear Y=%.3f | angular=%.3f\r\n",Car.TargetXVelocity, Car.TargetYVelocity, Car.TargetAngularVelocity);
}
}
void set_time_ref_cb(const void * msgin)
{
sensor_msgs__msg__TimeReference * msg = (sensor_msgs__msg__TimeReference*) msgin;
if(msg == NULL)
{
printf("[ERROR]received NULL time reference msg!\r\n");
}
else
{
RTC_TimeTypeDef rtc_time = {0};
RTC_DateTypeDef rtc_date = {0};
time_t timestamp_s = (time_t)msg->time_ref.sec;
struct tm *_tm = localtime(×tamp_s);
rtc_date.Year = _tm->tm_year-70;
rtc_date.Month = _tm->tm_mon;
rtc_date.Date = _tm->tm_mday;
rtc_time.Hours = _tm->tm_hour;
rtc_time.Minutes = _tm->tm_min;
rtc_time.Seconds = _tm->tm_sec;
rtc_time.SubSeconds = (uint32_t)(hrtc.Init.SynchPrediv-((msg->time_ref.nanosec/1000000000.0f)*(hrtc.Init.SynchPrediv+1.0f)));
HAL_RTC_SetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN);
HAL_RTC_SetTime(&hrtc, &rtc_time, RTC_FORMAT_BIN);
printf("[INFO] recv time. timestamp: (sec:%lu | nsec:%lu)\r\n",msg->time_ref.sec, msg->time_ref.nanosec);
}
}
/**
* @brief Micro-ROS parameter server callback function
*
* @param old_param Parameter actual value, `NULL` for new parameter request.
* @param new_param Parameter new value, `NULL` for parameter removal request.
* @param context User context, configured on `rclc_executor_add_parameter_server_with_context`.
* @return true
* @return false
*/
bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context)
{
(void) context;
if (old_param == NULL && new_param == NULL) {
printf("Callback error, both parameters are NULL\n");
return false;
}
if (old_param == NULL) {
printf("Creating new parameter %s\n", new_param->name.data);
} else if (new_param == NULL) {
printf("Deleting parameter %s\n", old_param->name.data);
} else {
printf("Parameter %s modified.", old_param->name.data);
switch (old_param->value.type) {
case RCLC_PARAMETER_BOOL:
printf(
" Old value: %d, New value: %d (bool)\r\n", old_param->value.bool_value,
new_param->value.bool_value);
break;
case RCLC_PARAMETER_INT:
printf(
" Old value: %ld, New value: %ld (int)\r\n", old_param->value.integer_value,
new_param->value.integer_value);
break;
case RCLC_PARAMETER_DOUBLE:
printf(
" Old value: %f, New value: %f (double)\r\n", old_param->value.double_value,
new_param->value.double_value);
break;
default:
break;
}
}
return true;
}Part2: freertos.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : freertos.c
* Description : Code for freertos applications
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* C standard library */
#include <stdio.h>
#include <time.h>
#include <math.h>
#include "arm_math.h"
/* STM32 Peripherals */
#include "rtc.h"
#include "tim.h"
/* Board Peripheral drivers */
#include "motor.h"
#include "led.h"
#include "icm20602.h"
#include "mecanum.h"
#include "microros.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define IMU_READY_SIGNAL (0x00000001)
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
/* system control states */
uint16_t ReleaseTime = 0; // unit:ms
/* USER CODE END Variables */
osThreadId defaultTaskHandle;
osThreadId Task_PublishIMUHandle;
osThreadId Task_PublishOdomHandle;
osThreadId Task_ExecuteSpinHandle;
osThreadId Task_PublishTFHandle;
osThreadId Task_PingAgentHandle;
osThreadId Task_PublishJointStatesHandle;
osTimerId Timer_MotorAdjustHandle;
osSemaphoreId MicroROSSemaphoreHandle;
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/*****************************/
/* port comm interface */
/*****************************/
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);
/*****************************/
/* memory management */
/*****************************/
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);
/* set wheel velocity subscriber callback function */
void set_wheel_vel_cb(const void * msgin);
/* set time reference subscriber call back function */
void set_time_ref_cb(const void * msgin);
/* parameter server callback function */
bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context);
/* mecanum constructure wheel odometry calculation function */
void Odometry_Update(nav_msgs__msg__Odometry *odom_msg,float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z);
/* auxiliary function of `Odometry_Update()` */
void euler_to_quat(float roll, float pitch, float yaw, float* q);
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void const * argument);
void PublishIMU(void const * argument);
void PublishOdom(void const * argument);
void ExecuteSpin(void const * argument);
void PublishTF(void const * argument);
void PingAgent(void const * argument);
void PublishJointStates(void const * argument);
void MotorAdjustcb(void const * argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );
/* GetTimerTaskMemory prototype (linked to static allocation support) */
void vApplicationGetTimerTaskMemory( StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize );
/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize )
{
*ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
*ppxIdleTaskStackBuffer = &xIdleStack[0];
*pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
/* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */
/* USER CODE BEGIN GET_TIMER_TASK_MEMORY */
static StaticTask_t xTimerTaskTCBBuffer;
static StackType_t xTimerStack[configTIMER_TASK_STACK_DEPTH];
void vApplicationGetTimerTaskMemory( StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize )
{
*ppxTimerTaskTCBBuffer = &xTimerTaskTCBBuffer;
*ppxTimerTaskStackBuffer = &xTimerStack[0];
*pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
/* place for user code */
}
/* USER CODE END GET_TIMER_TASK_MEMORY */
/**
* @brief FreeRTOS initialization
* @param None
* @retval None
*/
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* Create the semaphores(s) */
/* definition and creation of MicroROSSemaphore */
osSemaphoreDef(MicroROSSemaphore);
MicroROSSemaphoreHandle = osSemaphoreCreate(osSemaphore(MicroROSSemaphore), 1);
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */
/* Create the timer(s) */
/* definition and creation of Timer_MotorAdjust */
osTimerDef(Timer_MotorAdjust, MotorAdjustcb);
Timer_MotorAdjustHandle = osTimerCreate(osTimer(Timer_MotorAdjust), osTimerPeriodic, NULL);
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 800);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* definition and creation of Task_PublishIMU */
osThreadDef(Task_PublishIMU, PublishIMU, osPriorityAboveNormal, 0, 1024);
Task_PublishIMUHandle = osThreadCreate(osThread(Task_PublishIMU), NULL);
/* definition and creation of Task_PublishOdom */
osThreadDef(Task_PublishOdom, PublishOdom, osPriorityBelowNormal, 0, 1024);
Task_PublishOdomHandle = osThreadCreate(osThread(Task_PublishOdom), NULL);
/* definition and creation of Task_ExecuteSpin */
osThreadDef(Task_ExecuteSpin, ExecuteSpin, osPriorityNormal, 0, 1024);
Task_ExecuteSpinHandle = osThreadCreate(osThread(Task_ExecuteSpin), NULL);
/* definition and creation of Task_PublishTF */
osThreadDef(Task_PublishTF, PublishTF, osPriorityBelowNormal, 0, 1024);
Task_PublishTFHandle = osThreadCreate(osThread(Task_PublishTF), NULL);
/* definition and creation of Task_PingAgent */
osThreadDef(Task_PingAgent, PingAgent, osPriorityHigh, 0, 1024);
Task_PingAgentHandle = osThreadCreate(osThread(Task_PingAgent), NULL);
/* definition and creation of Task_PublishJointStates */
osThreadDef(Task_PublishJointStates, PublishJointStates, osPriorityBelowNormal, 0, 1024);
Task_PublishJointStatesHandle = osThreadCreate(osThread(Task_PublishJointStates), NULL);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* Suspend FreeRTOS tasks to avoid running micro-ros before initialization.
* Tasks are resumed after micro-ros initialization. */
osThreadSuspend(Task_PublishIMUHandle);
osThreadSuspend(Task_PublishOdomHandle);
osThreadSuspend(Task_ExecuteSpinHandle);
osThreadSuspend(Task_PublishTFHandle);
osThreadSuspend(Task_PingAgentHandle);
osThreadSuspend(Task_PublishJointStatesHandle);
/* USER CODE END RTOS_THREADS */
}
/* USER CODE BEGIN Header_StartDefaultTask */
/**
* @brief Function implementing the defaultTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
/* init code for USB_DEVICE */
......
microros_init();
microros_sync_time();
osThreadResume(Task_ExecuteSpinHandle);
osThreadResume(Task_PublishIMUHandle);
osThreadResume(Task_PublishOdomHandle);
osThreadResume(Task_PublishJointStatesHandle);
// osThreadResume(Task_PublishTFHandle);
// osThreadResume(Task_PingAgentHandle);
osTimerStart(Timer_MotorAdjustHandle,ENCODER_UPDATE_INTERVAL);
// osTimerStart(Timer_PingAgentHandle, 1000);
/* Infinite loop */
for(;;)
{
/* idle task for FreeRTOS memory management */
osDelay(500);
}
/* USER CODE END StartDefaultTask */
}
/* USER CODE BEGIN Header_PublishIMU */
/**
* @brief Function implementing the Task_PublishIMU thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PublishIMU */
void PublishIMU(void const * argument)
{
/* USER CODE BEGIN PublishIMU */
TickType_t xLastWakeTime;
const TickType_t xFrequency = 5;
xLastWakeTime = xTaskGetTickCount();
rcl_ret_t ret;
/* Infinite loop */
for(;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
osSemaphoreWait(MicroROSSemaphoreHandle,0);
ICM20602_UpdateMessage(&msg_imu);
ret = rcl_publish(&pub_imu,&msg_imu,NULL);
if(ret == RCL_RET_ERROR)
{
printf("[ERROR]pub imu failed.\r\n");
}
else if(ret == RCL_RET_INVALID_ARGUMENT)
{
printf("[ERROR]pub imu failed: Invalid arguments.\r\n");
}
else if(ret == RCL_RET_PUBLISHER_INVALID)
{
printf("[ERROR]pub imu failed: Invalid publisher.\r\n");
}
osSemaphoreRelease(MicroROSSemaphoreHandle);
}
/* USER CODE END PublishIMU */
}
/* USER CODE BEGIN Header_PublishOdom */
/**
* @brief Function implementing the Task_PublishOdom thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PublishOdom */
void PublishOdom(void const * argument)
{
/* USER CODE BEGIN PublishOdom */
rcl_ret_t ret;
rcutils_time_point_value_t current_time;
TickType_t xLastWakeTime;
const TickType_t xFrequency = 10;
xLastWakeTime = xTaskGetTickCount();
/* Infinite loop */
for(;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
Odometry_Update(&msg_odometry, (float)xFrequency/1000.0f, Car.CurrentXVelocity, Car.CurrentYVelocity, Car.CurrentAngularVelocity);
osSemaphoreWait(MicroROSSemaphoreHandle,100);
ret = rcutils_system_time_now(¤t_time);
if(ret != RCUTILS_RET_OK)
printf("[ERROR]read systime failed!\r\n");
msg_odometry.header.stamp.sec = RCUTILS_NS_TO_S(current_time);
msg_odometry.header.stamp.nanosec = (uint32_t)current_time+20000000;
// msg_odometry.header.stamp.sec = rmw_uros_epoch_millis();
// msg_odometry.header.stamp.nanosec = rmw_uros_epoch_nanos();
ret = rcl_publish(&pub_odometry, &msg_odometry, NULL);
osSemaphoreRelease(MicroROSSemaphoreHandle);
if(ret == RCL_RET_ERROR)
{
printf("[ERROR]publish odometry failed:publish error.\r\n");
}
else if(ret == RCL_RET_INVALID_ARGUMENT)
{
printf("[ERROR]publish odometry failed:invalid argument.\r\n");
}
else if(ret == RCL_RET_PUBLISHER_INVALID)
{
printf("[ERROR]publish odometry failed:publisher invalid.\r\n");
}
}
/* USER CODE END PublishOdom */
}
/* USER CODE BEGIN Header_ExecuteSpin */
/**
* @brief Function implementing the Task_ExecuteSpin thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ExecuteSpin */
void ExecuteSpin(void const * argument)
{
/* USER CODE BEGIN ExecuteSpin */
/* Infinite loop */
for(;;)
{
osSemaphoreWait(MicroROSSemaphoreHandle,100);
// Spin executor to receive messages
microros_spinsome();
osSemaphoreRelease(MicroROSSemaphoreHandle);
osDelay(10);
}
/* USER CODE END ExecuteSpin */
}
/* USER CODE BEGIN Header_PublishTF */
/**
* @brief Function implementing the Task_PublishTF thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PublishTF */
void PublishTF(void const * argument)
{
/* USER CODE BEGIN PublishTF */
rcl_ret_t ret;
rcutils_time_point_value_t current_time;
TickType_t xLastWakeTime;
const TickType_t xFrequency = 100;
xLastWakeTime = xTaskGetTickCount();
/* Infinite loop */
for(;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
osSemaphoreWait(MicroROSSemaphoreHandle,0);
ret = rcutils_system_time_now(¤t_time);
if(ret != RCUTILS_RET_OK)
printf("[ERROR]read systime failed!\r\n");
msg_tf2.transforms.data->header.stamp.sec = RCUTILS_NS_TO_S(current_time);
msg_tf2.transforms.data->header.stamp.nanosec = (uint32_t)current_time;
ret = rcl_publish(&pub_tf2,&msg_tf2,NULL);
if(ret != RCL_RET_OK)
{
printf("[ERROR]pub tf failed.\r\n");
}
osSemaphoreRelease(MicroROSSemaphoreHandle);
}
/* USER CODE END PublishTF */
}
/* USER CODE BEGIN Header_PingAgent */
/**
* @brief Function implementing the Task_PingAgent thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PingAgent */
void PingAgent(void const * argument)
{
/* USER CODE BEGIN PingAgent */
TickType_t xLastWakeTime;
const TickType_t xFrequency = 1000;
xLastWakeTime = xTaskGetTickCount();
/* Infinite loop */
for(;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
osSemaphoreWait(MicroROSSemaphoreHandle,0);
if(rmw_uros_ping_agent(100,1)!=RMW_RET_OK)
{
printf("[WARN] micro_ros_agent disconnected!\r\n");
/* NVIC MCU soft reset */
}
osSemaphoreRelease(MicroROSSemaphoreHandle);
}
/* USER CODE END PingAgent */
}
/* USER CODE BEGIN Header_PublishJointStates */
/**
* @brief Function implementing the Task_PublishJointStates thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PublishJointStates */
void PublishJointStates(void const * argument)
{
/* USER CODE BEGIN PublishJointStates */
rcl_ret_t ret;
rcutils_time_point_value_t current_time;
TickType_t xLastWakeTime;
const TickType_t xFrequency = 100;
xLastWakeTime = xTaskGetTickCount();
/* Infinite loop */
for(;;)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
joint_states.position[0] = LeftFrontMotor.CurrentAngle;
joint_states.position[1] = LeftRearMotor.CurrentAngle;
joint_states.position[2] = RightFrontMotor.CurrentAngle;
joint_states.position[3] = RightRearMotor.CurrentAngle;
// joint_states.velocity[0] = LeftFrontMotor.CurrentVelocity;
// joint_states.velocity[1] = LeftRearMotor.CurrentVelocity;
// joint_states.velocity[2] = RightFrontMotor.CurrentVelocity;
// joint_states.velocity[3] = RightRearMotor.CurrentVelocity;
ret = rcutils_system_time_now(¤t_time);
if(ret != RCUTILS_RET_OK)
printf("[ERROR]read systime failed!\r\n");
msg_odometry.header.stamp.sec =
msg_odometry.header.stamp.nanosec =
msg_joint_state.header.stamp.sec = RCUTILS_NS_TO_S(current_time);
msg_joint_state.header.stamp.nanosec = (uint32_t)current_time+20000000;
osSemaphoreWait(MicroROSSemaphoreHandle,0);
ret = rcl_publish(&pub_joint_states, &msg_joint_state, NULL);
osSemaphoreRelease(MicroROSSemaphoreHandle);
if(ret == RCL_RET_ERROR)
{
printf("[ERROR]publish /joint_states failed:publish error.\r\n");
}
else if(ret == RCL_RET_INVALID_ARGUMENT)
{
printf("[ERROR]publish /joint_states failed:invalid argument.\r\n");
}
else if(ret == RCL_RET_PUBLISHER_INVALID)
{
printf("[ERROR]publish /joint_states failed:publisher invalid.\r\n");
}
osDelay(5);
}
/* USER CODE END PublishJointStates */
}
/* MotorAdjustcb function */
void MotorAdjustcb(void const * argument)
{
/* USER CODE BEGIN MotorAdjustcb */
if(ReleaseTime == 0)
{
Car.TargetXVelocity = 0;
Car.TargetYVelocity = 0;
Car.TargetAngularVelocity = 0;
}
else
{
ReleaseTime -= ENCODER_UPDATE_INTERVAL;
}
Car_AdjustedVelocity(&Car);
/* USER CODE END MotorAdjustcb */
}
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void euler_to_quat(float roll, float pitch, float yaw, float* q)
{
float cy = arm_cos_f32(yaw * 0.5);
float sy = arm_sin_f32(yaw * 0.5);
float cp = arm_cos_f32(pitch * 0.5);
float sp = arm_sin_f32(pitch * 0.5);
float cr = arm_cos_f32(roll * 0.5);
float sr = arm_sin_f32(roll * 0.5);
q[0] = cy * cp * cr + sy * sp * sr;
q[1] = cy * cp * sr - sy * sp * cr;
q[2] = sy * cp * sr + cy * sp * cr;
q[3] = sy * cp * cr - cy * sp * sr;
}
void Odometry_Update(nav_msgs__msg__Odometry *odom_msg,float vel_dt, float linear_vel_x, float linear_vel_y, float angular_vel_z)
{
static float heading_ = 0.0f;
static float x_pos_ = 0.0f;
static float y_pos_ = 0.0f;
float delta_heading = angular_vel_z * vel_dt; //radians
float cos_h = arm_cos_f32(heading_);
float sin_h = arm_sin_f32(heading_);
float delta_x = (linear_vel_x * cos_h - linear_vel_y * sin_h) * vel_dt; //m
float delta_y = (linear_vel_x * sin_h + linear_vel_y * cos_h) * vel_dt; //m
//calculate current position of the robot
x_pos_ += delta_x;
y_pos_ += delta_y;
heading_ += delta_heading;
//calculate robot's heading in quaternion angle
//ROS has a function to calculate yaw in quaternion angle
float q[4];
euler_to_quat(0, 0, heading_, q);
//robot's position in x,y, and z
odom_msg->pose.pose.position.x = x_pos_;
odom_msg->pose.pose.position.y = y_pos_;
odom_msg->pose.pose.position.z = 0.0;
//robot's heading in quaternion
odom_msg->pose.pose.orientation.x = (double) q[1];
odom_msg->pose.pose.orientation.y = (double) q[2];
odom_msg->pose.pose.orientation.z = (double) q[3];
odom_msg->pose.pose.orientation.w = (double) q[0];
odom_msg->pose.covariance[0] = 0.001;
odom_msg->pose.covariance[7] = 0.001;
odom_msg->pose.covariance[35] = 0.001;
//linear speed from encoders
odom_msg->twist.twist.linear.x = linear_vel_x;
odom_msg->twist.twist.linear.y = linear_vel_y;
odom_msg->twist.twist.linear.z = 0.0;
//angular speed from encoders
odom_msg->twist.twist.angular.x = 0.0;
odom_msg->twist.twist.angular.y = 0.0;
odom_msg->twist.twist.angular.z = angular_vel_z;
odom_msg->twist.covariance[0] = 0.0001;
odom_msg->twist.covariance[7] = 0.0001;
odom_msg->twist.covariance[35] = 0.0001;
}
/* USER CODE END Application */Does it works if you use the micro-ROS Agent version according to your client distro? Instead of agent ROS2 distro: Foxy does it works if you use agent ROS2 distro: Humble?
Pansamic
commented
7 months ago I connect MCU to my PC(Ubuntu 22.04 ROS-Humble) and still the same - All publishers fail.
pablogs9
commented
7 months ago Could you debug inside this creation function in order to see which internal call is causing the error?
Pansamic
commented
7 months ago But this repo create a .a library to link. I cannot see micro-ros API inside.
publisher and subscriber all init succeed. Just publish fails.
pablogs9
commented
7 months ago You are using osSemaphoreWait on MicroROSSemaphoreHandle with a finite value and not checking the return code. This may lead to continue the execution without the ownership of the semaphore and probably calling rcl_publish from multiple threads simultaneously. Try to use an infinite value or checking the return code of the wait operation.
Pansamic
commented
7 months ago I set all to osSemaphoreWait(MicroROSSemaphoreHandle,0) to wait forever but still the same, all publishers fail.
Micro-ROS-Agent -v5 level output is like:
[1700228887.378333] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228887.378484] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228887.579297] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228887.579428] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228887.813349] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228887.813468] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.017290] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.017461] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.221211] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.221408] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.425279] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.425409] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.629239] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.629433] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.831246] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.831469] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228888.962705] debug | DataReader.cpp | read_fn | [==>> DDS <<==] | client_key: 0x00000000, len: 32
[1700228888.962788] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 44
[1700228888.963047] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228888.963094] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228889.163264] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228889.163403] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228889.360307] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228889.360447] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13
[1700228889.563251] debug | SerialAgentLinux.cpp | recv_message | [==>> SER <<==] | client_key: 0x71F88A29, len: 13
[1700228889.563390] debug | SerialAgentLinux.cpp | send_message | [** <<SER>> **] | client_key: 0x71F88A29, len: 13Idon't know what this 13bytes length data is, but it seems that agent is still communicating with micro-ros but publishers fail. If all publishers work well, there are all data with length 320(IMU) or 720(Odometry), etc.
pablogs9
commented
7 months ago Is 0 wait forever or a no timeout operation?
Pansamic
commented
7 months ago Thanks! I set all to osSemaphoreWait(MicroROSSemaphoreHandle,0XFFFFFFFF). Publishers works well in default ( reliable ) QoS now.
Here is the osSemaphoreWait() description.
/**
* @brief Wait until a Semaphore token becomes available
* @param semaphore_id semaphore object referenced with \ref osSemaphore.
* @param millisec timeout value or 0 in case of no time-out.
* @retval number of available tokens, or -1 in case of incorrect parameters.
* @note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS.
*/
int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec)
Background & Configuration
Detailed Description
I've realized multi-publisher with FreeRTOS after #125 issue solved. I use semaphore to protect each of micro-ros API. But it publishes messages with best-effort QoS, which are kind of denied by many ROS2 packages such as robot_state_publisher and slam_toolbox. So I change publisher init function from
rclc_publisher_init_best_effort()torclc_publisher_init_default(). Unfortunately, all of my publishers failed to publisher and returnRCL_RET_ERROR.Thinking
I've tried to subscribe what Micro-ROS publishes as ROS2 documentation c++ minimal subscriber does, But it cannot correctly call callback function, so it must be not receiving any message. Then I tried to change QoS of my subscriber to best-effort and it can receive messages.
Question