Function mainloop2

Modules/control/src/Utilities/terminal_control.cpp:287–625

}

void mainloop2()
{
    KeyboardEvent keyboardcontrol;
    Controller_Test Controller_Test;

    char key_now;
    char key_last;

    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
    cout << "ENTER key to control the drone: " <<endl;
    cout << "1 for Arm, Space for Takeoff, L for Land, H for Hold, 0 for Disarm, 8/9 for Trajectory tracking" <<endl;
    cout << "Move mode is fixed (XYZ_VEL,BODY_FRAME): w/s for body_x, a/d for body_y, k/m for z, q/e for body_yaw" <<endl;
    cout << "CTRL-C to quit." <<endl;

    while (ros::ok())
    {
        keyboardcontrol.RosWhileLoopRun();
        key_now = keyboardcontrol.GetPressedKey();
        switch (key_now)
        {

        //悬停, 应当只发送一次, 不需要循环发送
        case U_KEY_NONE:

          if (key_last != U_KEY_NONE)
          {
              //to be continued.
          }
          sleep(0.5);

          break;

        // 数字1（非小键盘数字）：解锁及切换到OFFBOARD模式
        case U_KEY_1:
          cout << " " <<endl;
          cout << "Arm and Switch to OFFBOARD." <<endl;
      
          Command_to_pub.header.stamp = ros::Time::now();
          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
          Command_to_pub.source = NODE_NAME;
          Command_to_pub.Reference_State.yaw_ref = 999;
          move_pub.publish(Command_to_pub);
          sleep(1.0);
          break;

        // 空格：起飞
        case U_KEY_SPACE:
          cout << " " <<endl;
          cout << "Switch to Takeoff Mode." <<endl;

          Command_to_pub.header.stamp = ros::Time::now();
          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
          Command_to_pub.Reference_State.yaw_ref = 0.0;
          Command_to_pub.source = NODE_NAME;
          move_pub.publish(Command_to_pub);