集群避障程序

集群避障节点orca_node

• 集群避障节点程序放置在Sunray_swarm/sunray_swarm/orca文件夹中，对应的程序有orca_node.cpp、orca.cpp、orca.h及ORCA算法库文件
• 该节点基于 ORCA（Optimal Reciprocal Collision Avoidance）算法实现了多智能体之间的协同避障和路径规划。该节点主要用于智能体的目标设置、避障计算、运动控制以及与 ROS 系统的交互。
• 启动文件截取如下：

<launch>
  <node pkg="sunray_swarm" name="orca_node" type="orca_node" output="screen">
        <rosparam command="load" file="$(find sunray_swarm)/config/rmtt_params.yaml" />
  </node>
</launch>

• orca_node参数

  • agent_type: 无人机类型，默认为1，不需要修改
  • agent_num: 智能体数量，默认为8，根据实际情况指定
  • agent_height: 无人机的指定飞行高度，默认为1.0米，所有的无人机飞行在指定高度，z轴暂不支持外部控制，建议设置为0.5-1.5之间

  ORCA参数：
  - todo

• **orca_node**订阅的话题

  • /sunray_swarm/{agent_name}/agent_state(sunray_msgs/agent_state): 订阅所有智能体状态，来自rmtt_control_node或ugv_control_node
  • /sunray_swarm/orca_cmd(sunray_msgs/orca_cmd): 订阅ORCA指令，来自地面站或Matlab
  • /sunray_swarm/{agent_name}/goal_point（geometry_msgs/Point）：订阅智能体目标点，来自地面站或者Matlab

• orca_node发布的话题（部分）

  • /sunray_swarm/agent_cmd （sunray_msgs/agent_cmd）：控制模式指令
  • /sunray_swarm/agent_orca_state（sunray_msgs/orca_state）：智能体ORCA状态，发送到地面站

• 关于话题**/sunray_swarm/orca_cmd**的说明

  • 该话题为自定义消息**orca_cmd.msg**，消息定义存放目录为Sunray_swarm/sunray_msgs/msg
  复制代码

  ## 自定义消息：ORCA算法控制指令
  
  ## 消息header
  std_msgs/Header header
  
  ## 控制指令来源
  string cmd_source
  
  ## ORCA算法指令
  uint8 orca_cmd   
  
  ## orca_cmd枚举
  uint8 SET_HOME = 0            ##必须要先发送SET_HOME指令才能开始ORCA算法运行
  uint8 RETURN_HOME = 1         ##返航至初始点
  uint8 SETUP_OBS = 2           ##设置障碍物
  uint8 ORCA_STOP = 3           ##ORCA算法暂停运行
  uint8 ORCA_RESTART = 4        ##ORCA算法继续运行
  
  uint8 ORCA_SCENARIO_1 = 11    ##ORCA程序预设目标点场景1
  uint8 ORCA_SCENARIO_2 = 12    ##ORCA程序预设目标点场景2
  uint8 ORCA_SCENARIO_3 = 13    ##ORCA程序预设目标点场景3
  uint8 ORCA_SCENARIO_4 = 14    ##ORCA程序预设目标点场景4
  uint8 ORCA_SCENARIO_5 = 15    ##ORCA程序预设目标点场景5
  
  ## 障碍物顶点，4个点围成一个凸体,针对SETUP_OBS模式
  geometry_msgs/Point[] obs_point    ## [m]

• 关于话题/sunray_swarm/agent_orca_state的说明

  • 该话题为自定义消息orca_state.msg，消息定义存放目录为Sunray_swarm/sunray_msgs/msg
  复制代码

  ## 自定义消息：ORCA算法状态
  ## 消息header
  std_msgs/Header header
  ## ORCA算法中智能体数量
  uint8 agent_num               
  ## 当前智能体ID
  uint8 agent_id               
  ## 当前智能体是否到达目标点
  bool arrived_goal
  ## 全部智能体是否到达目标点
  bool arrived_all_goal
  ## 当前智能体的目标位置
  float32[2] goal                 ## [m]
  ## 当前智能体的ORCA期望速度（ORCA算法计算得到）
  float32[2] vel_orca                 ## [m/s]