Promethues: swarm_control解读

标签可以对节点分组，具有 ns 属性，可以让节点归属某个命名空间
<group ns="iris_0">

    <group ns="iris_0">
        <!-- VINS-Fusion -->
        <node pkg="vins" type="vins_node" name="vins_estimator" args="$(find vins)/../config/xtdrone_sitl/iris_0_px4_sitl_config.yaml" />
        <include file="$(find vins)/launch/vins_rviz.launch" />
    </group>

`

Promethues

仿真节点图
实验节点图

Promethues代码功能解读

【Module/swarm_control】文件夹下
/ego_traj_to_cmd.cpp
订阅EGO的轨迹输出，即traj_server的输出，同时发布路径swarm_command指令至
swarm_controller.cpp，其实数据内容并没有做改变，只是换了一个存储格式

/swarm_controller.cpp
订阅集群控制指令（ego_traj_to_cmd.cpp提供）以及无人机姿态（swarm_estimator.cpp提供），或邻居无人机姿态（若存在多无人机）
发布无人机期望位置以及期望姿态，其中回调函数具体写在swarm_controller.h中
以计时器的方式执行mainloop_cb()函数，根据不同的无人机模式执行相应指令；以计时器的方式调用control_cb()函数，在无人机不同模式下，以不同方式发送期望位置或者速度。

/swarm_estimator.cpp
订阅px4的velocity_local数据（速度、角速度）以及imu数据（旋转角、线加速度、角加速度）；订阅实际实验中飞行高度，gazebo仿真中实验真值等
发布vision_pose/pose话题（无人机位置和偏航角），该话题是外部定位位置通过订阅的传感器经过处理发布的位置信息，发布无人机姿态drone_state话题给swarm_controller.cpp和swarm_ground_station.cpp，发布轨迹等消息
【参考】如何使用外部定位设备作为无人机位置估计https://bbs.amovlab.com/forum.php?mod=viewthread&tid=416

/swarm_ground_station.cpp
无人机群订阅swarm_command话题和drone_state话题，同时分别打印n驾无人机相关信息，包括位置、速度、加速度、无人机的状态信息等

/swarm_terminal_control.cpp
发布swarm_command话题（飞机控制指令，包括飞机模式，或直接飞行过去的目标点）和ego/goal话题，开启终端控制飞机，由用户决定飞机offboard->takeoff模式，同时用户输入飞行目标点，发布目标，由ego_replan_fsm.cpp接受

• mavros常用消息格式

https://zhuanlan.zhihu.com/p/364872655

• ROS与Navigation导航包用法详解

https://www.ncnynl.com/category/ROS2-Navigation2/
rostopic echo /uav2/prometheus/drone_state

---
header: 
  seq: 2539
  stamp: 
    secs: 279
    nsecs: 252000000
  frame_id: ''
connected: True
armed: True
landed: False
mode: "OFFBOARD"
odom_valid: False
time_from_start: 0.0
position: [3.0103416442871094, 2.974745512008667, 0.9297070503234863]
rel_alt: 0.0
velocity: [0.009897144511342049, -0.0017237744759768248, -0.009264364838600159]
attitude: [-0.009377377107739449, 0.014646852388978004, 0.2895106375217438]
attitude_q: 
  x: 0.00569589431254
  y: -0.00657036379321
  z: -0.144278819843
  w: -0.989498884845
attitude_rate: [-0.006956169381737709, -0.006956169381737709, -0.006956169381737709]
battery_state: 0.0

rostopic echo /prometheus/control_command
包括：起飞、降落、惯性系移动、机体系移动、速度控制、悬停、紧急停止

rostopic echo /uav2/prometheus/drone_state
包括：连接状态、解锁状态、飞行模式、位置、速度、姿态

header: 
  seq: 6019
  stamp: 
    secs: 453
    nsecs: 252000000
  frame_id: ''
connected: True
armed: True
landed: False
mode: "OFFBOARD"
odom_valid: False
time_from_start: 0.0
position: [2.9981884956359863, 2.9432859420776367, 0.9262572526931763]
rel_alt: 0.0
velocity: [0.015787186101078987, 0.012938245199620724, -0.0030638640746474266]
attitude: [-0.020875293761491776, 0.020600970834493637, 0.28966668248176575]
attitude_q: 
  x: 0.0118141648315
  y: -0.00868557101002
  z: -0.144418368059
  w: -0.989408058386
attitude_rate: [0.006223284639418125, 0.006223284639418125, 0.006223284639418125]
battery_state: 0.0

rostopic echo /uav2/map_generator/local_cloud

header: 
  seq: 13629
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "/world"
height: 1
width: 98
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 16
row_step: 1568
data: [236, 81, 192, 64, 61, 1....]

/uav2/prometheus/ego/traj_cmd

header: 
  seq: 445932
  stamp: 
    secs: 4632
    nsecs: 851000000
  frame_id: "world"
position: 
  x: 3.01908558629
  y: 3.00703613712
  z: 0.999371033507
velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
yaw: 0.289698575658
yaw_dot: 0.0
kx: [0.0, 0.0, 0.0]
kv: [0.0, 0.0, 0.0]
trajectory_id: 6
trajectory_flag: 1