Ubuntu14.04、indigo、大疆Onboard-SDK-ROS-3.6.1代码解读（二）

下面介绍飞行任务的代码执行过程

主要在 gps_callback() 回调函数里执行任务

/* 订阅 GPS 坐标回调函数 */    
void gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
  /* 设置初始时间为静态变量，用来计算每一次的间隔时长 */
  static ros::Time start_time = ros::Time::now();

  /* 计算间隔时长 */
  ros::Duration elapsed_time = ros::Time::now() - start_time;
  
  /* 设置的全局变量，用来接收 GPS 的信息 */
  /* msg 的值赋值给 current_gps */
  current_gps = *msg;

  /* 50HZ频率下采样 */
  if(elapsed_time > ros::Duration(0.02))
  {
    /* 20ms 后的时间 */
    start_time = ros::Time::now();

    /* 设置任务点的地方 */
    /* 判断任务状态 */
    switch(square_mission.state)
    {
      case 0:
        
        /* 所有任务结束，退出 */
        break;

      case 1:

        /* 如果任务 1 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {

          square_mission.step();
        }
        /* 任务 1 结束 */
        else
        {
          square_mission.reset();
            
          /* 以当前GPS、惯性系坐标作为此时的 ENU 参考系的坐标原点 */
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;

          /* 切换任务 2  */
          square_mission.setTarget(20, 0, 0, 0);
          square_mission.state = 2;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

      case 2:
        /* 如果任务 2 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;
          /* 切换任务 3 */
          square_mission.setTarget(0, -20, 0, 0);
          square_mission.state = 3;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 3:
        /* 如果任务 3 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;
          /* 切换任务 4 */
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 4:
        /* 如果任务 4 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          ROS_INFO("##### Mission %d Finished ....", square_mission.state);
          /* 切换任务 0 */
          square_mission.state = 0;
        }
        break;
    }
  }
}

这里补充以下C++关于指针和引用的知识

current_gps = *msg;

current_gps 为全局变量，在程序开始时候就开辟了内存空间

将 msg的值赋值给 current_gps，不是引用！！！、

current_gps 和 msg 的地址是不同的！！！

取值运算符：*

取地址运算符：&

/*  打印信息 */

print( &msg, *msg, ¤t_gps, *current_gps)

/* 输出 */

0x21

120212

0x54

120212

引用的本质

        1. 在c++ 内部实现其实是一个指针常量

举例：

int a = 10;

int &b = a;

相当于 int * const b = &a

         2. 引用没有重新定义一个新的变量，而是给已存在的变量取了一个别名，编译器不会为引用变量开辟新的内存空间，它和引用的变量共用一个内存空间。 

注意：

        1. 引用必须要初始化，必须要有值引用

        2. 引用一旦初始化后，就不可以更改

好，讲完了基础的c++知识，现在开始逐步分析代码

回调函数以50Hz的频率订阅GPS的消息

  /* 50HZ频率下采样 */
  if(elapsed_time > ros::Duration(0.02))

达到20ms后，更新开始的时间 start_time，用于下次计算时间间隔

   /* 20ms 后的时间 */
    start_time = ros::Time::now();

然后是重点任务部分

判断任务的状态，square_mission 在全局变量那里定义，后面会详细介绍 Mission 类。

 switch(square_mission.state)

程序完成初始化后，定义了一个初始任务

  /* 如果起飞 */
  if(takeoff_result)
  {
    /* 设置第一个任务点 */
    square_mission.reset();
    square_mission.start_gps_location = current_gps;
    square_mission.start_local_position = current_local_pos;
    square_mission.setTarget(0, 20, 3, 60);
    square_mission.state = 1;
    ROS_INFO("##### Start route %d ....", square_mission.state);
  }

此时的 square_mission.state = 1，当进入 gps_callback()回调函数时

进入case 1: 任务一

 case 1:
        /* 如果任务 1 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }

        /* 任务 1 结束 */
        else
        {
          /* 重置计数 */
          square_mission.reset();

          /* 更新GPS、惯性系坐标的初始参考点 */
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;

          /* 设置任务2的目标点 */
          square_mission.setTarget(20, 0, 0, 0);

          /* 切换任务状态为 2  */
          square_mission.state = 2;

          /* 开始执行 */
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

判断任务是否结束，如果任务没有结束，则继续执行step()函数(下一篇博客会介绍)

如果任务结束，则重置计数square_mission.reset();（包括 int inbound_counter、int outbound_counter、 int break_counter），这些分别代表inbound_counter 范围内计数，outbound_counter 范围外计数，break_counter 停止计数

/* 重置计数 */ 
square_mission.reset();

更新任务的初始参考点

/* 更新GPS、惯性系坐标的初始参考点 */
square_mission.start_gps_location = current_gps;
square_mission.start_local_position = current_local_pos;

设置任务的目标点

/* 设置任务2的目标点  */
square_mission.setTarget(20, 0, 0, 0);

设置任务状态为2

/* 切换任务状态为 2  */
square_mission.state = 2;

等待进入下一个任务。

当 square_mission.state = 2 时，下一次进入gps_callback() 函数

切换为case 2: 任务二

 case 2:
        /* 如果任务 2 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;
          /* 切换任务 3 */
          square_mission.setTarget(0, -20, 0, 0);
          square_mission.state = 3;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

判断任务二是否结束，没有的话继续执行任务二

如果任务二结束，跟任务一相同，执行重置，更新，设置目标点，切换状态过程，准备进入下一个任务三

当 square_mission.state = 3 时，下一次进入gps_callback() 函数

切换为case 3: 任务三

case 3:
        /* 如果任务 3 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.start_local_position = current_local_pos;
          /* 切换任务 4 */
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

执行情况跟任务一、二相同，完成任务后

切换到case 4: 任务四

case 4:
        /* 如果任务 4 没有结束，就执行 step() 函数 */
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          ROS_INFO("##### Mission %d Finished ....", square_mission.state);
          /* 任务结束 */
          square_mission.state = 0;
        }
        break;

当 square_mission.state = 3 时，所有任务结束，将 square_mission.state 设为0。

切换到case 0: 任务结束

 case 0:
        /* 任务结束 */
        break;

整个任务执行的过程就是：1 → 2 → 3 → 4 → 0

任务切换通过标志位 square_mission.state 来设定。