px4ctrl代码解读-px4ctrl_node

头文件

#include <ros/ros.h>
#include "PX4CtrlFSM.h"
#include <signal.h>

1.初始化节点

    ros::init(argc, argv, "px4ctrl");
    ros::NodeHandle nh("~");            //初始化句柄
    signal(SIGINT, mySigintHandler);    //触发退出终端
    ros::Duration(1.0).sleep();         

2.初始化参数和类

    Parameter_t param;                  //Parameter_t在PX4CtrlFSM.h中被开放
    param.config_from_ros_handle(nh);   //用read_essential_param函数读取参数->使用getParam将参数读取到节点（初始化）
    
    // Controller controller(param);
    LinearControl controller(param);    //LinearControl在contrller.h->LinearControl类中被开放，形参Parameter_t
    PX4CtrlFSM fsm(param, controller);  

3.订阅相关话题和服务

    ros::Subscriber state_sub =
        nh.subscribe<mavros_msgs::State>("/mavros/state",
                                         10,
                                         boost::bind(&State_Data_t::feed, &fsm.state_data, _1));
    //订阅mavros连接状态
    ros::Subscriber extended_state_sub =
        nh.subscribe<mavros_msgs::ExtendedState>("/mavros/extended_state",
                                                 10,
                                                 boost::bind(&ExtendedState_Data_t::feed, &fsm.extended_state_data, _1));
    //订阅mavros连接状态
    ros::Subscriber odom_sub =
        nh.subscribe<nav_msgs::Odometry>("odom",
                                         100,
                                         boost::bind(&Odom_Data_t::feed, &fsm.odom_data, _1),
                                         ros::VoidConstPtr(),
                                         ros::TransportHints().tcpNoDelay());
    //订阅里程计
    ros::Subscriber cmd_sub =
        nh.subscribe<quadrotor_msgs::PositionCommand>("cmd",
                                                      100,
                                                      boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1),
                                                      ros::VoidConstPtr(),
                                                      ros::TransportHints().tcpNoDelay());
    //cmd暂时不知道谁发布的
    ros::Subscriber imu_sub =
        nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", // Note: do NOT change it to /mavros/imu/data_raw !!!
                                       100,
                                       boost::bind(&Imu_Data_t::feed, &fsm.imu_data, _1),
                                       ros::VoidConstPtr(),
                                       ros::TransportHints().tcpNoDelay());
    //订阅飞控imu数据
    ros::Subscriber rc_sub;
    //.........................................省略.................................................//
    //下面是建立服务
    fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
    //.........................................省略.................................................//

4.如果设置禁用遥控器则警告，未设置则err直至有信号输入

    if (param.takeoff_land.no_RC)
    {
        ROS_WARN("PX4CTRL] Remote controller disabled, be careful!");
    }
    else
    {
        ROS_INFO("PX4CTRL] Waiting for RC");
        while (ros::ok())
        {
            ros::spinOnce();
            if (fsm.rc_is_received(ros::Time::now()))
            {
                ROS_INFO("[PX4CTRL] RC received.");
                break;
            }
            ros::Duration(0.1).sleep();
        }
    }
  
 5.检查ros状态及mavros连接状态，直至连接成功.执行 fsm.process()

    while (ros::ok() && !fsm.state_data.current_state.connected)
    {
        ros::spinOnce();
        ros::Duration(1.0).sleep();
        if (trials++ > 5)
            ROS_ERROR("Unable to connnect to PX4!!!");
    }

    ros::Rate r(param.ctrl_freq_max);
    while (ros::ok())
    {
        r.sleep();
        ros::spinOnce();
        fsm.process(); // We DO NOT rely on feedback as trigger, since there is no significant performance difference through our test.
    }

    return 0;
}

State_Data_t在input中定义current_state变量类型为 mavros_msgs::State

class State_Data_t
{
public:
  mavros_msgs::State current_state;
  mavros_msgs::State state_before_offboard;

  State_Data_t();
  void feed(mavros_msgs::StateConstPtr pMsg);
};