UAVControl—Prometheus代码阅读笔记【二】——节点实现

Prometheusv2中的uav_control代码阅读笔记，个人学习用。
内容正在补充中，欢迎大家交流！

代码来源：
https://github.com/amov-lab/Prometheus

---

---

（一）无人机控制节点：uav_control_main_1

1.源文件位置

• Modules/uav_control/src/uav_control_node.cpp
• Modules/uav_control/src/uav_controller.cpp
• Modules/uav_control/src/uav_estimator.cpp

节点名称为uav_control_main_1，对应的ROS功能包名为prometheus_uav_control。该模块为Prometheus项目的基础模块，完成Prometheus其他功能模块与mavros节点进行数据交互。
uav_control_node.cpp完成UAV_estimator与UAV_controller类的实例化，具体功能在uav_controller.cpp与uav_estimator.cpp中实现。
发布话题和订阅话题中均带有命名空间/uav*，无人机编号不同则对应的命名空间不同，一般情况下默认为/uav1。

2.发布话题

• /uav*/mavros/setpoint_raw/attitude(mavros_msgs/AttitudeTarget)：期望的无人机姿态控制量
• /uav*/mavros/setpoint_raw/global(mavros_msgs/GlobalPositionTarget)：期望的无人机经纬高控制量
• /uav*/mavros/setpoint_raw/local(mavros_msgs/PositionTarget)：期望的无人机ENU坐标系的控制量(包含位置、速度、加速度等)
• /uav*/mavros/vision_pose/pose(geometry_msgs/PoseStamped)：MAVROS提供的外部定位数据接口
• /uav*/prometheus/control_state(prometheus_msgs/UAVControlState)：Prometheus控制状态
• /uav*/prometheus/odom(nav_msgs/Odometry)：无人机里程计数据
• /uav*/prometheus/offset_pose(prometheus_msgs/OffsetPose)：ENU坐标系下的位置偏移量
• /uav*/prometheus/state(prometheus_msgs/UAVState)：无人机状态合集，包括位置\速度\姿态\模式等
• /uav*/prometheus/text_info(prometheus_msgs/TextInfo)：反馈到地面站的打印信息
• /uav*/prometheus/trajectory(nav_msgs/Path)：无人机运动轨迹
• /uav*/prometheus/uav_mesh(visualization_msgs/Marker)：无人机位置(带图标)，用于RVIZ显示

3.订阅话题

• /uav*/mavros/battery(sensor_msgs/BatteryState)：无人机电池状态
• /uav*/mavros/global_position/global(sensor_msgs/NavSatFix)：无人机当前经纬度数据，经飞控融合过后的数据.
• /uav*/mavros/gpsstatus/gps*/raw(mavros_msgs/GPSRAW)：GPS模块数据状态
• /uav*/mavros/imu/data(sensor_msgs/Imu)：IMU模块数据
• /uav*/mavros/local_position/pose(geometry_msgs/PoseStamped)：无人机ENU坐标系下的位置数据
• /uav*/mavros/local_position/velocity_local(geometry_msgs/TwistStamped)：无人机ENU坐标系的当前速度数据
• /uav*/mavros/setpoint_raw/target_attitude(mavros_msgs/AttitudeTarget)：PX4中无人机的姿态设定值 (坐标系:ENU系)
• /uav*/mavros/setpoint_raw/target_local(mavros_msgs/PositionTarget)：PX4中无人机的位置/速度/加速度设定值(坐标系:ENU系)
• /uav*/mavros/state(mavros_msgs/State)：无人机当前状态(PX4)
• /uav*/prometheus/command(prometheus_msgs::UAVCommand)：无人机控制指令(用于COMMAND_CONTROL模式)
• /uav*/prometheus/fake_rc_in(mavros_msgs/RCIn)：PX4遥控器数据
• /uav*/prometheus/offset_pose(prometheus_msgs::OffsetPose)：ENU坐标系下的位置偏移量
• /uav*/prometheus/set_local_offset_pose(prometheus_msgs::OffsetPose)：设置ENU坐标系下无人机的位置偏移量
• /uav*/prometheus/setup(prometheus_msgs/UAVSetup)：无人机设置指令
• /uav*/prometheus/state(prometheus_msgs/UAVState)：无人机状态合集，包括位置、速度、姿态、模式等

4.参数

• uav_id: 无人机编号
• sim_mode：是否为仿真（true为仿真，false非仿真）
• control/pos_controller：控制器标志位
• control/enable_external_control：是否使用外部控制器
• control/Takeoff_height：command_control模式下起飞高度
• control/Disarm_height：land_control模式下自动上锁的高度
• control/Land_speed：land_control模式下下降的速度
• control/location_source：定位数据来源（备注: 0: MOCAP， 1: T265， 2: GAZEBO， 3: FAKE_ODOM， 4: GPS， 5: RTK， 6: UWB）
• control/maximum_safe_vel_xy：XY轴最大安全速度
• control/maximum_safe_vel_z：Z轴最大安全速度
• control/maximum_vel_error_for_vision：最大vision/px4速度误差
• geo_fence/x_min：地理围栏X轴最小值
• geo_fence/x_max：地理围栏X轴最大值
• geo_fence/y_min：地理围栏Y轴最小值
• geo_fence/y_max：地理围栏Y轴最大值
• geo_fence/z_min：地理围栏Z轴最小值
• geo_fence/z_max：地理围栏Z轴最大值

uav_control_indoor.ymal 中可修改

（二）遥控器节点：joy_node

1.源文件位置

• Modules/uav_control/utils/joy_node.cpp

节点名称为joy_node，在joy_node功能包中实现。对应的ROS功能包名为prometheus_uav_control。

2.发布话题

• /uav*/prometheus/fake_rc_in(mavros_msgs/RCIn)：发布相同的遥控器指令
• /uav*/mavros/rc/override(mavros_msgs/OverrideRCIn)：无用
• /uav*/mavros/manual_control/send(mavros_msgs/ManualControl)：无用

3.订阅话题

• joy/set_feedback：无用