PX4_sender

变量声明

float cur_time;         //程序运行时间
float Takeoff_height;   //默认起飞高度
float Disarm_height;    //自动上锁高度
float Land_speed;       //降落速度
int Land_mode;          //降落策略选择
Eigen::Vector2f geo_fence_x;        //地理围栏
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;
Eigen::Vector3d Takeoff_position;   // 起飞位置
prometheus_msgs::DroneState _DroneState;    //无人机状态量
prometheus_msgs::ControlCommand Command_Now;    //无人机当前执行命令
prometheus_msgs::ControlCommand Command_Last;   //无人机上一条执行命令

Eigen::Vector3d state_sp(0,0,0);
Eigen::Vector3d state_sp_extra(0,0,0);
double yaw_sp;
double yaw_rate_sp;
prometheus_msgs::Message message;
prometheus_msgs::LogMessageControl LogMessage;

//RVIZ显示：期望位置
geometry_msgs::PoseStamped ref_pose_rviz;
float dt = 0;
ros::Publisher rivz_ref_pose_pub;
ros::Publisher message_pub;
ros::Publisher log_message_pub;

订阅

// 【订阅】为任务模块生成的控制指令(来自terminal_control.cpp)
ros::Subscriber Command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10, Command_cb);
// 本话题为Prometheus地面站发送的控制指令
ros::Subscriber station_command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command_station", 10, station_command_cb);
//【订阅】无人机状态(来自px4_pos_estimator.cpp)
ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, drone_state_cb);

发布

// 【发布】参考位姿 RVIZ显示用
rivz_ref_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/control/ref_pose_rviz", 10);
// 【发布】用于地面站显示的提示消息
message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);
// 【发布】用于log的消息
log_message_pub = nh.advertise<prometheus_msgs::LogMessageControl>("/prometheus/log/control", 10);

参数

// 参数读取
nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
nh.param<float>("Disarm_height", Disarm_height, 0.15);
nh.param<float>("Land_speed", Land_speed, 0.2);
nh.param<int>("Land_mode",Land_mode,0);
nh.param<float>("geo_fence/x_min", geo_fence_x[0], -100.0);
nh.param<float>("geo_fence/x_max", geo_fence_x[1], 100.0);
nh.param<float>("geo_fence/y_min", geo_fence_y[0], -100.0);
nh.param<float>("geo_fence/y_max", geo_fence_y[1], 100.0);
nh.param<float>("geo_fence/z_min", geo_fence_z[0], -100.0);
nh.param<float>("geo_fence/z_max", geo_fence_z[1], 100.0);
// 设定起飞位置
Takeoff_position[0] = 0.0;
Takeoff_position[1] = 0.0;
Takeoff_position[2] = 0.15;
// 建议控制频率: 10 - 50Hz, 控制频率取决于控制形式，若控制方式为速度或加速度应适当提高频率
ros::Rate rate(20.0);

用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】

command_to_mavros _command_to_mavros;

// 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系 本话题来自飞控
position_target_sub = command_nh.subscribe<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/target_local", 10, &command_to_mavros::pos_target_cb,this);
// 【订阅】无人机期望角度/角速度 坐标系:ENU系 本话题来自飞控
attitude_target_sub = command_nh.subscribe<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/target_attitude", 10, &command_to_mavros::att_target_cb,this);

// 【发布】位置/速度/加速度期望值至飞控 坐标系 ENU系
advertise<mavros_msgs::PositionTarget>(uav_name + "/mavros/setpoint_raw/local", 10);
// 【发布】角度/角速度期望值至飞控 坐标系 ENU系
setpoint_raw_attitude_pub = command_nh.advertise<mavros_msgs::AttitudeTarget>(uav_name + "/mavros/setpoint_raw/attitude", 10);
// 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量及油门推力注意这里是NED系的！！
actuator_setpoint_pub = command_nh.advertise<mavros_msgs::ActuatorControl>(uav_name + "/mavros/actuator_control", 10);
// 【发布】本话题要发送至飞控(通过Mavros_extra功能包 /plugins/mount_control.cpp发送)
mount_control_pub = command_nh.advertise<mavros_msgs::MountControl>(uav_name + "/mavros/mount_control/command", 1);

主循环

• Idle

• Takeoff

• Hold

• Land

• Move

• Disarm