ROS学习笔记之八：尝试在FreeRTOS中使用ROSSerial

我理解 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函数的返回值，请看客留意！)