我理解 ROSSerial只是为MCU提供了一个和基于ROS主机（上位机）通讯的方式，而MCU要实现的功能还是要自己编写实现，这部分和以往需求一样，那么采用什么程序框架就是不可回避的问题。
现在由于STM32以极优的性价比提供了可以运行RTOS的硬件平台，使用RTOS已经不是一件奢侈的事情了，如果能在RTOS框架下使用ROSSerial应该是编写嵌入式控制程序的福音，故做此尝试。
为了便于实施，也便于围观者理解，还是基于前面的程序修改。
具体构思如下：
1、构建以下任务：
A）定时检测 rosserial 信息，并控制 LED 闪烁
B）自检任务，执行一次后消除
C）执行命令任务，等待ROS消息，根据消息执行。
2、创建队列消息：
在 rosserial 的 Callback 函数中产生队列消息，将收到的动作命令送给执行命令任务。
因为 Callback 函数应该是在中断中产生的，故使用 xQueueSendFromISR()
消息队列设计为 2个单元，每个单元 3 字节 ：电机命令 PWM 舵机角度
按此定义消息单元数据类型：
typedefstruct
{
uint8_t u8MotorCmd;
uint8_t u8Pwm;
uint8_t u8ServoVal;
}command_type;
三个任务如下：
// 任务 A：定时检测 rosserial 消息，控制 LED 闪烁
static voidtaskA(void* arg)
{
UNUSED(arg);
pinMode(LED_BUILTIN,OUTPUT);
g_bLED_On = true;
digitalWrite(LED_BUILTIN,HIGH);
gc_i16DisplayTimeCnt = DISPLAY_TIME;
while (1)
{
// Sleep for 10 milliseconds.
vTaskDelay((10L * configTICK_RATE_HZ) /1000L);
if(g_bSelfTest == false)
{
nh.spinOnce(); // 检测rosserial
gc_i16DisplayTimeCnt--; // LED Display
if(gc_i16DisplayTimeCnt <= 0)
{
gc_i16DisplayTimeCnt = DISPLAY_TIME;
g_bLED_On = !g_bLED_On;
digitalWrite(LED_BUILTIN,g_bLED_On);
}
}
}
}
这样编写逻辑似乎更清晰了。
// 任务 B：自检
static voidtaskB(void* arg)
{
UNUSED(arg);
uint8_t i,j;
g_bSelfTest = true;
for(i = 0; i< MAX_CMD_NO; i++)
{
j = 0;
myServo.writeMicroseconds(correctServo(ga_u8TestCommand[i*4+ j]));
j++;
vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L); // 延时 1000ms, 等待舵机操作完成
YM3.setRunDistance(ga_u8TestCommand[i*4+ j]);
j++;
YM3.run(ga_u8TestCommand[i*4 +j],ga_u8TestCommand[i*4 + j+1]);
while(YM3.getMotorStat() ==ga_u8TestCommand[i*4 + j])
{
vTaskDelay((10L* configTICK_RATE_HZ) / 1000L); // 延时 10ms ，每10ms查询一下电机执行是否完成
}
}
g_bSelfTest = false;
vTaskDelete(NULL);
}
看客可以比较一下原来的自检处理逻辑，是不是这个直观了许多？
// 任务 C：执行命令
static voidtaskC(void* arg)
{
UNUSED(arg);
while(1)
{
if(g_bSelfTest== false)
{
xQueueReceive(xQueueCommand,&g_sCommand,portMAX_DELAY);
if(g_sCommand.u8ServoVal!= g_ucServoVal)
{
g_ucServoVal= g_sCommand.u8ServoVal;
myServo.writeMicroseconds(correctServo(g_ucServoVal));
vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L); // 延时 1000ms, 等待舵机操作完成
}
driveMotor(g_sCommand.u8MotorCmd,g_sCommand.u8Pwm);
vTaskDelay((3000L* configTICK_RATE_HZ) / 1000L); // 运行 3000ms,
driveMotor(BRAKE,0); // 停止运行
}
else
{
vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L); // 延时 1000ms
}
}
}
任务执行也比原来更容易理解，尤其是原来定义的每个命令执行3s的处理，此处用RTOS的延时命令就很直观的处理了，看上去程序在此死等，实际上RTOS已经挂起此任务，执行别的任务（taskA），以保证控制逻辑正确。这就是使用RTOS的优势，调度交给系统了。
初始化（setup）中修改如下：
……..
// ------ 190612 ---------------
xQueueCommand =xQueueCreate(QUEUE_ITEM_NUM,QUEUE_ITEM_SIZE);
// create taskA at priority 2
h_taskA = xTaskCreate(taskA, NULL,configMINIMAL_STACK_SIZE, NULL, 2, NULL);
// create taskB at priority 2
h_taskB = xTaskCreate(taskB, NULL, configMINIMAL_STACK_SIZE,NULL, 2, NULL);
// create taskC at priority 2
h_taskC = xTaskCreate(taskC, NULL,configMINIMAL_STACK_SIZE, NULL, 2, NULL);
// check for creation errors
if (xQueueCommand == NULL || h_taskA !=pdPASS || h_taskB != pdPASS || h_taskC != pdPASS)
{
Serial.println(F("Creationproblem"));
while(1);
}
// start scheduler
vTaskStartScheduler();
while(1);
}
相应的消息接收处理回调函数修改为：
voidoneWheel_cb( const geometry_msgs::Twist& cmd_msg)
{
int temp;
static portBASE_TYPE xHigherPriorityTaskWoken;
command_type sCommand;
if(cmd_msg.linear.x == 0.0)
{
sCommand.u8MotorCmd= BRAKE;
sCommand.u8Pwm= 0;
}
else
{
if(cmd_msg.linear.x > 0.0)
{
sCommand.u8MotorCmd = FORWARD;
sCommand.u8Pwm = byte(cmd_msg.linear.x);
}
else
{
sCommand.u8MotorCmd = BACKWARD;
sCommand.u8Pwm = byte(0 - cmd_msg.linear.x);
}
}
temp = int(cmd_msg.angular.z);
if(temp <0)
{
temp = 0;
}
if(temp >180)
{
temp = 180;
}
sCommand.u8ServoVal = byte(temp);
xQueueSendFromISR(xQueueCommand,&sCommand,&xHigherPriorityTaskWoken);
if(xHigherPriorityTaskWoken == pdTRUE)
{
// **任务切换
}
}
上面那个“**任务切换”部分还没吃透，暂时空着，其主要作用是在发送队列消息后，启动一次系统任务调度处理，以便等待此消息的任务可以立刻响应，这个在uCOS中是系统自己处理的，在系统的中断服务程序退出时，会自己启动一次任务切换处理。在 FreeRTOS中好像需要显性的操作一下，这个等实际编程时再琢磨。
此时原来Arduino的loop函数没有作用了：
// WARNINGidle loop has a very small stack (configMINIMAL_STACK_SIZE)
// loopmust never block
void loop(){
// Not used.
}
程序运行全部由FreeRTOS管理了，在Arduino程序中只是体现在这一句：
#include<STM32FreeRTOS.h> // 190612
应该说还是比在MDK中编写简化了很多。
上述程序编写完成后，只是修改了一些语法错误，程序就实现了原来的功能，编译后资源占用如下：
这比上一个程序占用的程序空间大了不少，用了约30k字节，即便这样，应该还是有足够的空间编写功能程序的。
如果真的遇到功能需求复杂，换一个MCU即可，STM32有充足的选择空间，支持STM32Duino的MCU也很多。
至此，已经验证了使用STM32+Arduino环境可以实现 ROS系统的外围功能硬件的构建，下一步可以回归到 ROS，看看如何使用程序实现通过 ROSSerial控制小车。现在测试使用的还是命令行发送。
不过这一关打通了，为后续ROS学习提供了足够的素材，否则我看网上很多人尝试ROS都是基于有限的几个平台，没有变化，关键是成本颇高，削弱了很多人的参与激情。
注：程序附在 QQ 群 32438885 的共享文件夹中，供参考。
(程序重点是测试这样是否可行，没有严格处理RTOS函数的返回值，请看客留意！)