代码阅读 on ZJUDancer

Behavior 行为控制模块

Mon, 01 Jan 0001 00:00:00 +0000

老文档：Behavior module

Overview

dancer-behavior 是我们的策略模块，其主要由以下组件组成：

btree: 行为树 (Behavior Tree)，是我们策略的高层抽象
core: 基于 btree 模块，并且与我们机器人的实际相结合的底层抽象
role: 机器人的角色，如 Striker、Defender 等
skill: 机器人的技能，包含了头部与身体的技能。如 SeekBall、Kick 等
util: 一些实用函数或者封装

整个策略模块的运转流程如下：

初始化，读取 Parameter Server 中的参数，并设定初始 Role
通过 ROS Topic 的订阅，接收视觉、步态、裁判盒以及队友的信息，并保存于 DBlackboard 中
若下位机已经开机，则根据当前的 Role 与 DBlackboard 中的信息，执行对应的 Skill。
Skill 在执行完毕之后，会产生对应的身体动作 BodyCommand 与头部动作 HeadCommand，两者合成 ActionCommand，并通过 ROS Topic 发送给步态模块 dancer-motion
一个运行周期结束，重复步骤 1-4

补充说明

Voronoi 区域计算
通过 voronoi_region 属性计算机器人在场上的最优区域，用于辅助团队协作与位置分配。
团队协作
通过 update_team_info 和 check_team_info 方法实现团队信息共享与任务分配，提升整体协同效率。
状态管理
通过 set_state 方法管理不同的行为状态，以支持机器人在不同场景下的动作切换。
异常处理
对下位机连接状态、球的可见性等关键运行信息进行监控和处理，确保系统运行的稳定性与可靠性。

Behavior Tree

📘 参考 Wiki：
Behavior Trees Wiki（点击查看） btree 是该文章的完整实现。在 Wiki 中需要重点关注的概念主要有：

Behavior 行为控制模块（新版草稿）

Mon, 01 Jan 0001 00:00:00 +0000

老文档：Behavior module

Overview

dancer-behavior 是我们的策略模块，其主要由以下组件组成：

btree: 行为树 (Behavior Tree)，是我们策略的高层抽象
core: 基于 btree 模块，并且与我们机器人的实际相结合的底层抽象
role: 机器人的角色，如 Striker、Defender 等
skill: 机器人的技能，包含了头部与身体的技能。如 SeekBall、Kick 等
util: 一些实用函数或者封装

整个策略模块的运转流程如下：

初始化，读取 Parameter Server 中的参数，并设定初始 Role
通过 ROS Topic 的订阅，接收视觉、步态、裁判盒以及队友的信息，并保存于 DBlackboard 中
若下位机已经开机，则根据当前的 Role 与 DBlackboard 中的信息，执行对应的 Skill
Skill 在执行完毕之后，会产生对应的身体动作 BodyCommand 与头部动作 HeadCommand，两者合成 ActionCommand，并通过 ROS Topic 发送给步态模块 dancer-motion
一个运行周期结束，重复步骤 1-4

Behavior Tree

参照 Wiki。btree 是该文章的完整实现。

在 Wiki 需要关注的概念主要有：

Leaf task: Action 与 Condition
Branch task: Sequence、Selector 与 Parallel
Decorator

了解了基本概念之后，可以看 dancer-behavior/src/dbehavior/btree 中的示例。

Planner 路径规划模块

Mon, 01 Jan 0001 00:00:00 +0000

先明确这些变量的意义：

代码块

bool see_ball= false;
vector<double> ball_field = {1,0,0}; // 球在机器人坐标系下的位置
vector<double> ball_global = {0,0,0}; // 球在全局坐标系下的位置
vector<double> ball_velocity = {0,0,0}; // 球在机器人坐标系下的速度
vector<double> robot_global = {0,0,0}; // 机器人全局坐标系下的位置
vector<double> action_global = {0,0,0}; // 目标点在全局坐标系下的位置
vector<double> vel_cmd = {0,0,0}; // 规划出来，用于发布的速度
std::vector<std::vector<double>> obstacles; // 障碍物信息，每个障碍物包含 x, y, radius，存储多个障碍物
int gait_type = 0; // 步态类型
int kick_type = 0; // 踢球类型
bool kick_flag = false; // 是否处于踢球准备阶段的标志
bool dribble_flag = false; // 是否处于盘球阶段的标志
double ball_field_distance; //球在机器人坐标系下的距离
double ball_field_angle; //球在机器人坐标系下的角度
double robot_angle_error = 0; // 朝向目标点 - 现在的朝向

先看主函数：

1. 节点初始化与 ROS 通信配置 (Line 413 - 434)

环境变量区分机器人：通过读取环境变量 ZJUDANCER_ROBOTID，为话题 (Topic) 加上特定的前缀，从而在一台电脑或仿真环境中区分控制不同的机器人个体。
有个DWA的实例，但还没用上
发布与订阅 (Pub & Sub)：
- 订阅视觉信息 VisionInfo：获取机器人全局坐标、球的局部和全局坐标、球速等。
- 订阅行为指令 ActionCommand：接收上层决策层下发的指令（包含目标坐标 bodyCmd.x, y, t 以及期望的步态类型 gait_type）。
- 发布速度指令 cmd_vel (geometry_msgs::Twist)：发布计算后的平移和旋转速度控制底盘（全向移动底盘）。
- 发布手柄指令 /joy_msg：通过模拟手柄行为来发送诸如“左脚踢球”、“右脚踢球”、“向左扑救”等特殊动作触发指令。

2. 控制参数初始化 (Line 436 - 470)

定义了一系列控制动作时使用的常量，包括：

Vision 视觉处理模块

Mon, 01 Jan 0001 00:00:00 +0000

一、代码功能框架（包含主要功能）

注：本部分仅对代码功能进行概括，所提到的关键方法将在下一部分进行详细解读

核心模块

相机输入

ZED 相机
GstCamera（GStreamer驱动）
- 管道初始化（ init() ）
- 缓冲区回调（ onBuffer() ）
- 图像尺寸获取（ GetWidth() ）
V4L2Camera（备选驱动，已弃用，仅作参考）
输入源适配（图像文件夹/视频文件）

帧处理与投影

图像数据封装（Frame）
逆投影映射（IPM）
- 3D+2D投影（ project() ）
- 2D+3D反投影（ inverseProject() ）
坐标转换与航向矫正（Projection）
- 航向偏移计算（ CalcHeadingOffset() ）

目标检测

基类 IDetector （统一接口）
中心圆检测（CircleDetector）
- 卡尔曼滤波优化（ Update() ）
- 场地坐标输出（ result_circle() ）
障碍物检测（ObstacleDetector）
- 二值图生成（ GetBinary() ）
- 障碍物特征提取（ Process() ）
角点检测（CornerDetector）
- 交叉点分类（ classifyIntx() ）
- 定位特征生成（ LandMark ）
球检测（BallDetector）
球门检测（GoalDetector）
线段分类（LineClassifier）
- 航向偏差计算（ CalYawBias() ）

定位与导航

AMCL
- 粒子集管理（SampleSet）
- KdTree聚类（ ClusterNode() ）
Tracker（目标跟踪）
- 相机角度控制（ Process() ）

控制中枢

多线程调度

step1：图像捕获与预处理
step2：检测与定位

ROS通信

订阅运动、行为信息
发布视觉结果、图像
服务：AMCL控制等

状态管理

位姿计算
初始化（ InitVisionYaw() ）

辅助部分

配置参数

工具类

测试程序

二、代码功能解读（包含主要类与关键方法）

注：以下对关键方法的解读不包含对函数所需传参的解读，只列出函数名

Network 网络通信模块

Mon, 01 Jan 0001 00:00:00 +0000

1. 模块概述

dnetwork 是整个机器人系统（ZJUDancer）的网络通信中枢。

对内（ROS 节点）： 它是各个本地算法模块（视觉 Vision、动作 Motion、行为 Behavior）的信息聚合器和分发器。
对外（UDP 网络）： 它是机器人与外界（队友机器人、裁判盒、外部监控端）进行物理数据包交换的唯一接口。

该模块高度依赖底层的两个基础库：

dprocess： 提供多线程和定时循环调度的能力。
dtransmit： 提供对底层 UDP Socket 的高度封装（异步接收、回调绑定、组播/单播发送）。

2. 目录结构与核心入口

dnetwork 的核心文件包含头文件与源文件两部分，核心依赖标准 RoboCup 的协议头文件：

dnetwork/
├── include/dnetwork/
│ ├── RoboCup/RoboCupGameControlData.h # 国际 RoboCup 官方定义的裁判盒数据结构
│ ├── gamecontroller.hpp # 裁判盒通信类头文件
│ └── team.hpp # 队内通信类头文件
└── src/
 ├── gamecontroller.cpp
 ├── main.cpp # ROS 节点入口
 └── team.cpp

2.1 模块启动 (`main.cpp`)

作为独立的 ROS 节点，程序入口非常简洁。初始化 ROS 后，实例化 Team 和 GameController。由于它们均继承自 dprocess::DProcess，调用 spin() 后会分别在独立的子线程中启动网络监听和定时发送的循环，最后主线程通过 join() 阻塞等待。

IO 输入输出模块

Mon, 01 Jan 0001 00:00:00 +0000

概述

dio (dancer-io) 是一个以 ROS 节点 (IntegratedNode) 运行的状态汇聚与指令转换模块。它负责监听机器人的传感器数据与运动指令，在内部进行里程计解算和稳定性评估，并向外发布综合运动状态（MotionInfo）和关节控制指令（JointState）。

整个模块的运转流程如下：

状态感知：读取 IMU 数据和头部电机的当前位置与速度。
稳定性评估：根据 IMU 的 Roll/Pitch 角度判断机器人是否稳定，并记录前倾或后倾状态。
里程计解算：结合 /cmd_vel 的线速度与 IMU 的偏航角（Yaw），计算机器人在平面内的相对坐标。
指令桥接与保护：接收行为模块下发的头部动作指令，在机器人稳定时进行角度单位转换并透传；在失稳时强制输出固定的保护姿态角度。
状态广播：将综合状态打包为 MotionInfo 并发布，同时广播里程计 TF 变换。

核心功能架构

dio 模块的内部逻辑主要分为三个核心子系统：状态与稳定性监测、里程计解算、指令桥接与保护。

graph TD classDef L1 fill:#2B3A35,color:#fff,stroke:#333,stroke-width:1px; classDef L2 fill:#6C9A8C,color:#fff,stroke:#333,stroke-width:1px; classDef L3 fill:#A3C3B2,color:#000,stroke:#333,stroke-width:1px; classDef L4 fill:#D8D8D8,color:#000,stroke:#333,stroke-width:1px; DIO[dio 模块 IntegratedNode] --> StateMonitor[状态与稳定性监测] DIO --> OdomCalc[里程计解算] DIO --> CmdBridge[指令桥接与保护] StateMonitor --> IMUData[IMU 姿态解析] StateMonitor --> FallDetect[跌倒检测与恢复] OdomCalc --> VelInteg[速度积分] OdomCalc --> TFPub[TF 坐标系发布] CmdBridge --> HeadCtrl[头部关节控制] CmdBridge --> SafeMode[跌倒保护姿态] class DIO L1; class StateMonitor,OdomCalc,CmdBridge L2; class IMUData,FallDetect,VelInteg,TFPub,HeadCtrl,SafeMode L3;

1. 状态与稳定性监测

该部分checkStability通过订阅 /imu/data 提取欧拉角（Roll, Pitch, Yaw）。

Config 配置管理模块

Mon, 01 Jan 0001 00:00:00 +0000

一、概述

dconfig 是我们的集中配置仓库，承担两项核心职责：

静态常量定义：通过 C++ 头文件 dconstant.hpp 提供编译期可用的场地几何参数与网络参数。
动态配置热更新：通过 Python 脚本 config_watchdog.py 监听配置文件目录，在 YAML 文件发生变化时自动调用 rosparam load 并向对应模块发布通知。

模块的目录结构如下：

dconfig/
├── include/dconfig/
│ └── dconstant.hpp # 编译期常量（场地几何 + 网络参数）
├── launch/
│ └── dconfig.launch # 启动 config_watchdog 节点
├── matlab/ # MATLAB 脚本
├── scripts/
│ └── config_watchdog.py # 配置热更新守护进程
├── global.yml # 全局运行参数
└── 1/ 2/ 3/ 4/ 5/ 6/ 10/ # 各机器人个体配置目录
 ├── dancer_io/
 ├── dmotion/
 └── dvision/

二、静态常量：`dconstant.hpp`

该头文件使用 #pragma once 保护，定义了两个命名空间下的编译期 const 常量。

Launch 启动脚本模块

Mon, 01 Jan 0001 00:00:00 +0000

概述

dlaunch 模块通过一系列 .launch 文件，将 dvision、dmotion、dbehavior、dnetwork、dancer_io 等子系统进行组合。从代码实现来看，该模块依赖 ROS 原生的 <include>、<node>、<param>、<rosparam> 以及 <env> 标签来组织节点启动与参数加载逻辑。

核心启动文件

根据文件内部的标签结构与包含关系，可将这些启动文件划分为以下几类：

1. 纯模块组合 (`<include>` 主导)

这类文件主要通过 <include> 标签加载其他包的启动文件，不显式声明具体的节点（<node>）参数：

game.launch：包含 dvision/launch/default.launch、dancer_io/launch/motion_io.launch（game.launch中有引用，但robocup_ws中未直接包含）、dmotion/launch/motion_hub.launch （类似的，未直接包含源文件）以及 dnetwork/launch/default.launch。
piplus.launch：包含 dvision/launch/zed_default.launch、dio/launch/dancer-io.launch、dplanner/launch/default.launch、dnetwork/launch/default.launch 以及 dbehavior/launch/default.launch。
mv.launch：包含 dconfig/dconfig.launch、dvision/launch/default.launch 和 dmotion/launch/default.launch。
sim_default.launch：包含 dvision/launch/default.launch、dmotion/launch/default.launch、dnetwork/launch/default.launch，并设置参数 /ZJUDancer/Simulation 为 true。
get_image_180.launch：包含 dconfig/launch/default.launch、dmotion/launch/default.launch、dvision/launch/default.launch、dnetwork/launch/default.launch，并加载 global.yml。

2. 仿真实例与集群编排

通过参数服务器注入 <param name="/ZJUDancer/Simulation" value="true"/>，并组合特定编号的文件：

单体实例 (sim_1.launch - sim_6.launch)：分别包含 dvision、dmotion 和 dnetwork 目录下对应编号（1-6）的 .launch 文件，同时设置 /ZJUDancer/BroadcastMVInfo 为 true。
集群组合 (2.launch, 3.launch, 4.launch)：通过嵌套包含多个 sim_X.launch 文件来实现组合。例如 4.launch 内部直接包含了 sim_1.launch 到 sim_4.launch。

3. 数据录制与回放 (`rosbag`)

利用 rosbag 节点处理数据流，并通过参数标记状态：

代码阅读 on ZJUDancer

Behavior 行为控制模块

Overview

Behavior Tree

Behavior 行为控制模块（新版草稿）

Overview

Behavior Tree

Planner 路径规划模块

先明确这些变量的意义：

先看主函数：

1. 节点初始化与 ROS 通信配置 (Line 413 - 434)

2. 控制参数初始化 (Line 436 - 470)

Vision 视觉处理模块

一、代码功能框架（包含主要功能）

核心模块

相机输入

帧处理与投影

目标检测

定位与导航

控制中枢

多线程调度

ROS通信

状态管理

辅助部分

配置参数

工具类

测试程序

二、代码功能解读（包含主要类与关键方法）

Network 网络通信模块

1. 模块概述

2. 目录结构与核心入口

2.1 模块启动 (main.cpp)

IO 输入输出模块

概述

核心功能架构

1. 状态与稳定性监测

Config 配置管理模块

一、概述

二、静态常量：dconstant.hpp

Launch 启动脚本模块

概述

核心启动文件

1. 纯模块组合 (<include> 主导)

2. 仿真实例与集群编排

3. 数据录制与回放 (rosbag)

2.1 模块启动 (`main.cpp`)

二、静态常量：`dconstant.hpp`

1. 纯模块组合 (`<include>` 主导)

3. 数据录制与回放 (`rosbag`)