大疆Onboard-SDK-ROS-3.6.1代码解读（二）

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

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

    
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;
  
  
  
  current_gps = *msg;

  
  if(elapsed_time > ros::Duration(0.02))
  {
    
    start_time = ros::Time::now();

    
    
    switch(square_mission.state)
    {
      case 0:
        
        
        break;

      case 1:

        
        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;

          
          square_mission.setTarget(20, 0, 0, 0);
          square_mission.state = 2;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

      case 2:
        
        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;
          
          square_mission.setTarget(0, -20, 0, 0);
          square_mission.state = 3;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 3:
        
        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;
          
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 4:
        
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          ROS_INFO("##### Mission %d Finished ....", square_mission.state);
          
          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的消息

  
  if(elapsed_time > ros::Duration(0.02))

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

   
    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:
        
        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;

          
          square_mission.setTarget(20, 0, 0, 0);

          
          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();

更新任务的初始参考点

square_mission.start_gps_location = current_gps;
square_mission.start_local_position = current_local_pos;

设置任务的目标点

square_mission.setTarget(20, 0, 0, 0);

设置任务状态为2

square_mission.state = 2;

等待进入下一个任务。

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

切换为case 2: 任务二

 case 2:
        
        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;
          
          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:
        
        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;
          
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

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

切换到case 4: 任务四

case 4:
        
        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 来设定。