ZBreak
约 4434 字
相关信息
此 Skill 是马赛回旋的实现,由 ZAttackV2 调用,他会绕球旋转,尝试绕死对面,但是在两方跑相同代码的时候,会死在里面,两个车跳二人转
该脚本会在上层计算好每帧的动作,下发到 SmartGotoPosition ,并绕过 RTT 路径规划,由 PRMPathPlanner 提供路径规划服务
Utils::isBallInFace
/**
* @name: isBallInFace
* @description: 判断机器人是否正在持球,综合视觉和红外传感器两路信号来提高可靠性
* @param {CVisionModule*} pVision
* @param {int} id 机器人编号
* @param {double} car_to_ball_dist 机器人中心到球心的标准距离
* @param {double} buffer 距离容差
* @return {*}
*/
bool isBallInFace(const CVisionModule* pVision,int id,double car_to_ball_dist,double buffer){
double PI=PARAM::Math::PI;
CGeoPoint ball_pos=pVision->ball().Pos();
CGeoPoint me_pos=pVision->ourPlayer(id).RawPos();
bool inface;
double me_dir=pVision->ourPlayer(id).RawDir();
double me_to_ball_dir=(ball_pos-me_pos).dir();
double me_to_ball_dist=(ball_pos-me_pos).mod();
CGeoPoint face=me_pos+Utils::Polar2Vector(car_to_ball_dist,pVision->ourPlayer(id).Dir());
bool frared=RobotSensor::Instance()->IsInfraredOn(id);
double dist=car_to_ball_dist+buffer;
if(pVision->ball().Valid() && dist<PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER+70){
// 球有效 且 dist 过小
// 防止阈值太小漏判
dist=PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER+70;
}//0.18
else if(!pVision->ball().Valid()){// 球丢失
dist=PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER+100;
}
// 其他正常情况,使用传入参数
int fraredoff=RobotSensor::Instance()->fraredOff(id);
int fraredon=RobotSensor::Instance()->fraredOn(id);
static int vision_on=0;
static int vision_off=0;
if(me_to_ball_dist< dist && fabs(Normalize(me_to_ball_dir-me_dir))<PI/3){
// 球在判定半径内
// 角度:球在机器人正面 ±60° 扇形范围内
vision_on++;
vision_off=0;
}else{
vision_on=0;
vision_off++;
}
if(vision_on>20 || ((fraredoff<5 ||fraredon>2)&& (vision_on>1 || vision_off<5))){
inface= true;
// 当满足以下任一条件:inface = true
// 条件 A:vision_on > 20
// → 视觉连续 20 帧以上确认持球(纯视觉高置信度)
// 条件 B:(fraredoff < 5 || fraredon > 2) && (vision_on > 1 || vision_off < 5)
// ├─ 红外部分:fraredoff < 5(红外未持续断开)
// │ OR fraredon > 2(红外持续触发超过 2 帧)
// └─ 视觉辅助:vision_on > 1(视觉刚开始确认)
// OR vision_off < 5(视觉刚失效,不超过 5 帧)
}
else{
inface= false;
if(fraredon>1){
// RobotSensor::Instance()->robotMsg[id].infrared=false;
}
}
return inface;
}读取 [](file:///home/lkh/robocup/rocos2026/lumina/main/Medusa/src/Utils/utils.cpp#1009-1009),行 1009 到 1050
函数签名
bool isBallInFace(const CVisionModule* pVision, int id, double car_to_ball_dist, double buffer)| 参数 | 含义 |
|---|---|
pVision | 视觉模块,提供球和机器人的实时位置 |
id | 机器人编号 |
car_to_ball_dist | 机器人中心到球心的标准距离(通常为 PLAYER_CENTER_TO_BALL_CENTER) |
buffer | 距离容差,扩大判定区域 |
第一步:基础数据获取(第1011-1014行)
me_dir // 机器人当前朝向角
me_to_ball_dir // 机器人→球的方向角
me_to_ball_dist // 机器人→球的距离第二步:动态调整判定距离阈值(第1015-1022行)
double dist = car_to_ball_dist + buffer;对 dist 做三档保护:
| 情况 | dist 取值 | 原因 |
|---|---|---|
球有效 且 dist 过小 | PLAYER_CENTER_TO_BALL_CENTER + 70 | 防止阈值太小漏判 |
| 球丢失(无效) | PLAYER_CENTER_TO_BALL_CENTER + 100 | 球消失时放宽距离,靠红外维持判断 |
| 其他正常情况 | car_to_ball_dist + buffer | 使用传入参数 |
第三步:视觉帧计数(第1023-1030行)
if (me_to_ball_dist < dist && fabs(Normalize(me_to_ball_dir - me_dir)) < PI/3)同时满足两个条件:
- 距离:球在判定半径内
- 角度:球在机器人正面 ±60° 扇形范围内
满足 → vision_on++,vision_off = 0
不满足 → vision_on = 0,vision_off++
vision_on/off是static变量,跨帧累积,起到时间滤波作用,防止单帧噪声误判。
第四步:双源融合最终判定(第1031-1043行)
inface = true 当满足以下任一条件:
条件 A:vision_on > 20
→ 视觉连续 20 帧以上确认持球(纯视觉高置信度)
条件 B:(fraredoff < 5 || fraredon > 2) && (vision_on > 1 || vision_off < 5)
├─ 红外部分:fraredoff < 5(红外未持续断开)
│ OR fraredon > 2(红外持续触发超过 2 帧)
└─ 视觉辅助:vision_on > 1(视觉刚开始确认)
OR vision_off < 5(视觉刚失效,不超过 5 帧)设计思路总结
视觉信号 ──┐
├──► 融合判断 ──► 是否持球
红外信号 ──┘- 视觉单独判定:需要连续 20 帧,延迟较高但可靠
- 红外辅助判定:响应快,但容易受噪声干扰,因此要求视觉在 5 帧内配合确认
- 两者配合:既保证实时响应,又避免单点故障导致误判,是典型的多传感器融合策略
初始化
void CZBreak::plan(const CVisionModule *pVision){
if(pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE * 0.1){
grabMode = ANTI_ROT;
last_mode = ANTI_ROT;
judge_once = true;
move_Point = CGeoPoint(-9999*10,-9999*10);
dribblePoint = CGeoPoint(-9999*10,-9999*10);
cycleCenter = CGeoPoint(-9999*10,-9999*10);
start_playerPos = CGeoPoint(-9999*10,-9999*10);
final_Pos = CGeoPoint(-9999*10,-9999*10);
global_angle=-99999;
aim_time = 0;
radio=0;
}
const MobileVisionT& ball = pVision->ball();
CGeoPoint passTarget = task().player.pos;
int vecNumber = task().executor;
const PlayerVisionT& me = pVision->ourPlayer(vecNumber);
int oppNum = ZSkillUtils::instance()->getTheirBestPlayer();
const PlayerVisionT& enemy = pVision->theirPlayer(oppNum);
double power = task().player.kickpower;
const CVector me2Ball = ball.Pos() - me.RawPos();
const CVector me2enemy = enemy.Pos() - me.RawPos();
const CVector me2target = passTarget - me.RawPos();
CGeoPoint final_target = passTarget;
CGeoPoint real_target = passTarget;
bool needBreakThrough = false;
bool holding_ball=Utils::isBallInFace(pVision,vecNumber,PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,34);
// 综合视觉和红外传感器两路信号,确定我是否拿球
dribblePoint = RobotSensor::Instance()->dribblePoint(vecNumber);
// 吸球杆与球接触的位置正在优化工具选择...这段代码是 CZBreak::plan() 函数的初始化与状态准备部分,具体完成以下工作:
超时重置(第76-86行):若当前帧与上一帧时间差超过 0.1 秒(即技能中断后重新进入),将所有状态变量重置为初始值,防止脏数据影响新一轮执行。
基本数据读取(第87-97行):从视觉模块获取球、己方机器人(
me)、最佳对手(enemy)的状态,以及目标传球点和踢球力度。常用向量计算(第99-101行):预计算
me→ball、me→enemy、me→target三个常用方向向量,供后续逻辑使用。持球与颠球点检测(第110-112行):
holding_ball:检测机器人是否正在持球(红外传感器)dribblePoint:吸球杆与球接触的位置
状态判断(第113行):调用
judge_state()根据敌人位置决定grabMode(ANTI_ROT原地旋转突破 /SIDE_MOVE侧向绕行)记录上一状态(第115行):
last_mode = grabMode,供后续帧做状态转换检测。
总结:这段代码是每帧执行时的数据准备与模式决策入口,为后续的运动目标点计算(calc_point)和射门逻辑(shoot_module)提供必要的上下文信息。
judge_state
judge_state(pVision, vecNumber, passTarget, dribblePoint, grabMode);
last_mode = grabMode;/**
* @name: judge_state
* @description: 在技能第一帧(judge_once == true)决定持球突破的策略模式
* ——是原地旋转躲避(ANTI_ROT)还是侧向绕圆绕过障碍(SIDE_MOVE)
* @param {CVisionModule*} pVision
* @param {int} vecNumber
* @param {CGeoPoint} &target
* @param {int} &grabmode
* @return {*}
*/
void judge_state(const CVisionModule* pVision,const int vecNumber,CGeoPoint &target,const CGeoPoint&dribblePoint,int &grabmode){
const PlayerVisionT&me = pVision->ourPlayer(vecNumber);
const MobileVisionT&ball = pVision->ball();
const PlayerVisionT& bestene=pVision->theirPlayer(Utils::BestNearEne(ball.Pos(),pVision));
double mindist = 99999;
CVector dribblepos2enemy = bestene.RawPos() - dribblePoint;
CVector dribblepos2target = target - dribblePoint;
CGeoPoint recvPos = MessiDecision::Instance()->receiverPos();
// 构造两条线段,用于检测敌人是否挡路
CGeoSegment me2recvPosLine = CGeoSegment(me.RawPos()+Utils::Polar2Vector(100,(recvPos - me.RawPos()).dir()),recvPos);
// 我→接球者 的线段
CGeoSegment me2targetLine = CGeoSegment(me.RawPos()+Utils::Polar2Vector(100,(target - me.RawPos()).dir()),target);
// 我→目标 的线段
std::vector<CGeoPoint> enemy_points;
// 收集距球 CHECK_OBSTCLE_DIST 范围内、且不在对方禁区内的所有敌人
for(int i = 0; i < PARAM::Field::MAX_PLAYER; i++){
auto test_enemy = pVision->theirPlayer(i);
if(test_enemy.Valid() && (test_enemy.Pos()-dribblePoint).mod() < CHECK_OBSTCLE_DIST && !Utils::InTheirPenaltyArea(test_enemy.Pos(),0)) enemy_points.push_back(test_enemy.Pos());
if(test_enemy.Valid()&&me2recvPosLine.IsPointOnLineOnSegment(me2recvPosLine.projection(test_enemy.Pos()))){
if(mindist < me2recvPosLine.dist2Point(test_enemy.Pos())) mindist = me2recvPosLine.dist2Point(test_enemy.Pos());
}
}
if(judge_once){
// 只执行一次的决策
judge_once = false;
if(Utils::Normalize(dribblepos2target.dir() - dribblepos2enemy.dir()) < 0) rot_d = 1;
// 敌人在目标左侧,向右绕
else rot_d = -1;
// 敌人在目标右侧,向左绕
// if(Utils::InTheirPenaltyArea(dribblePoint,LATERAL_MOVE_DIFF)&&!ShootModule::Instance()->canShoot(pVision,vecNumber)&&mindist<130)
// grabmode = LATERAL_MOVE;
// return;
for(auto test_point:enemy_points){
// 遍历障碍敌人,判断是否需要 SIDE_MOVE
// if(Utils::InTheirPenaltyArea(target,0)) break;
if(me2targetLine.IsPointOnLineOnSegment(me2targetLine.projection(test_point))){
// 敌人是否投影在"我→目标"线段上
if(me2targetLine.dist2Point(test_point)<130) {// 到该线段距离 < 130mm
// 若满足(路径被封堵),进入 SIDE_MOVE 模式计算:
radio = dribblePoint.dist(bestene.Pos())+90>600?600:dribblePoint.dist(bestene.Pos())+180;
// 绕圆半径 radio,敌人越近,圆越小;最大不超过 600mm
final_Pos = dribblePoint + Utils::Polar2Vector(radio,(target-dribblePoint).dir());
// 圆弧终点 final_Pos,沿目标方向偏移 radio,即绕过敌人后的落脚点
start_playerPos = me.RawPos();
CVector me2finalpos = final_Pos - me.RawPos();
CGeoPoint foot_point = me.RawPos() + Utils::Polar2Vector(me2finalpos.mod()/2.0,me2finalpos.dir());
// foot_point = me→final_Pos 中点
cycleCenter = CGeoLine(dribblePoint,target).projection(foot_point);
// 旋转圆心 cycleCenter = 将 foot_point 投影到"dribblePoint→target"连线上
// if(Utils::InTheirPenaltyArea(cycleCenter,radio+90))
if(!Utils::IsInField(cycleCenter,radio+180)){
// 圆心超出场地的边界情况
CVector cycle2me = me.RawPos() - cycleCenter;
double roll_Dir1 = cycle2me.dir() + PARAM::Math::PI*2/(4.5);
double roll_Dir2 = cycle2me.dir() - PARAM::Math::PI*2/(4.5);
CGeoPoint point1 = cycleCenter + Utils::Polar2Vector(radio,roll_Dir1);
CGeoPoint point2 = cycleCenter + Utils::Polar2Vector(radio,roll_Dir2);
// 计算圆弧在两个方向上的候选点 point1、point2
GDebugEngine::Instance()->gui_debug_arc(point1,30,0,360);
GDebugEngine::Instance()->gui_debug_arc(point2,30,0,360);
// 根据圆心在场地四个象限的位置,选择更靠近场内的方向修正 rot_d,
// 若两个方向都出界则直接 break(放弃 SIDE_MOVE)。
// 后面的内容都在做这些
if(cycleCenter.x()>0){
if(PARAM::Field::PITCH_LENGTH/2-cycleCenter.x()<radio+180){
if(cycleCenter.y()>0){
if(PARAM::Field::PITCH_WIDTH/2-cycleCenter.y()<radio+180){
break;
}else{
if(point1.x()>point2.x()) rot_d = -1;
else rot_d = 1;
}
}else{
if(cycleCenter.y()+PARAM::Field::PITCH_WIDTH/2<radio+180){
break;
}else{
if(point1.x()>point2.x()) rot_d = -1;
else rot_d = 1;
}
}
}else{
if(cycleCenter.y()>0){
if(point1.y()>point2.y()) rot_d = -1;
else rot_d = 1;
}else{
if(point1.y()<point2.y()) rot_d = -1;
else rot_d = 1;
}
}
}else{
if(PARAM::Field::PITCH_LENGTH/2+cycleCenter.x()<radio+180){
if(cycleCenter.y()>0){
if(PARAM::Field::PITCH_WIDTH/2-cycleCenter.y()<radio+180){
break;
}else{
if(point1.x()<point2.x()) rot_d = -1;
else rot_d = 1;
}
}else{
if(cycleCenter.y()+PARAM::Field::PITCH_WIDTH/2<radio+180){
break;
}else{
if(point1.x()<point2.x()) rot_d = -1;
else rot_d = 1;
}
}
}else{
if(cycleCenter.y()>0){
if(point1.y()>point2.y()) rot_d = -1;
else rot_d = 1;
}else{
if(point1.y()<point2.y()) rot_d = -1;
else rot_d = 1;
}
}
}
}
grabmode = SIDE_MOVE;
return;
}
}
}
// 无障碍时,原地旋转对准目标即可
grabmode = ANTI_ROT;
return;
}
}总体作用: 在技能第一帧(judge_once == true)决定持球突破的策略模式——是原地旋转躲避(ANTI_ROT)还是侧向绕圆绕过障碍(SIDE_MOVE)。
第一步:数据准备(第197-210行)
CVector dribblepos2enemy = bestene.RawPos() - dribblePoint; // 球口→最近敌人
CVector dribblepos2target = target - dribblePoint; // 球口→目标
// 构造两条线段,用于检测敌人是否挡路
me2recvPosLine // 我→接球者 的线段
me2targetLine // 我→目标 的线段
enemy_points // 收集距球口 CHECK_OBSTCLE_DIST 范围内、且不在对方禁区内的所有敌人第二步:只执行一次的决策(judge_once 保护)
2.1 决定初始旋转方向 rot_d(第218-219行)
if (Normalize(dribblepos2target.dir() - dribblepos2enemy.dir()) < 0)
rot_d = 1; // 敌人在目标左侧,向右绕
else
rot_d = -1; // 敌人在目标右侧,向左绕2.2 遍历障碍敌人,判断是否需要 SIDE_MOVE(第224-296行)
对每个 enemy_points 中的敌人,判断:
敌人是否投影在"我→目标"线段上 && 到该线段距离 < 130mm若满足(路径被封堵),进入 SIDE_MOVE 模式计算:
① 计算绕圆半径 radio
radio = min(dribblePoint.dist(bestene) + 180, 600)
// 敌人越近,圆越小;最大不超过 600mm② 计算圆弧终点 final_Pos
final_Pos = dribblePoint + Polar2Vector(radio, 目标方向)
// 沿目标方向偏移 radio,即绕过敌人后的落脚点③ 计算旋转圆心 cycleCenter
foot_point = me→final_Pos 中点
cycleCenter = 将 foot_point 投影到"dribblePoint→target"连线上即圆心在"持球到目标"连线上,确保绕圆轨迹最终指向目标。
④ 处理圆心超出场地的边界情况
若 cycleCenter 距场边过近(!IsInField(cycleCenter, radio+180)),则计算圆弧在两个方向上的候选点 point1、point2,根据圆心在场地四个象限的位置,选择更靠近场内的方向修正 rot_d,若两个方向都出界则直接 break(放弃 SIDE_MOVE)。
⑤ 设置模式并返回
grabmode = SIDE_MOVE;
return;2.3 无障碍时(第295-297行)
grabmode = ANTI_ROT; // 无敌人挡路,原地旋转对准目标即可流程总结
judge_state()
│
├─ 收集附近敌人
│
└─ judge_once(仅第一帧执行)
│
├─ 有敌人挡在我→目标路径上(dist < 130mm)?
│ ├─ YES → 计算 cycleCenter、radio、final_Pos
│ │ 处理边界 → 设置 rot_d
│ │ grabmode = SIDE_MOVE(弧线绕过)
│ │
│ └─ NO → grabmode = ANTI_ROT(原地转体突破)
│
└─ 后续帧不再重新判断,保持模式稳定judge_once 的设计保证了模式在整个突破过程中不频繁抖动,只在技能刚激活时做一次决策。
执行
/**************** set subTask*********************************/
TaskT grabTask(task());
if (VERBOSE)
GDebugEngine::Instance()->gui_debug_msg(
(dribblePoint + Utils::Polar2Vector(25 * 10, -PARAM::Math::PI) +
Utils::Polar2Vector(106 * 10, PARAM::Math::PI / 2)),
"break", COLOR_CYAN);
move_Point = calc_point(pVision, vecNumber, passTarget, dribblePoint, grabMode);
// 计算运动目标点
// 根据 grabMode 决定机器人本帧应移动到哪里:
// grabMode | move_Point 含义
// ----------------------------------------
// ANTI_ROT | 原地不动(me.RawPos()),只旋转朝向
// SIDE_MOVE | 圆弧上的下一个目标点(绕圆前进)
// 同时 calc_point 还会更新全局 global_angle(目标朝向角)。
if (VERBOSE)
GDebugEngine::Instance()->gui_debug_arc(move_Point, 150, 0, 360, COLOR_GREEN);
grabTask.player.pos = move_Point;
grabTask.player.angle = global_angle;
grabTask.player.flag =
PlayerStatus::NOT_AVOID_OUR_VEHICLE | PlayerStatus::NOT_AVOID_THEIR_VEHICLE;
// 不避车,就干转
grabTask.player.max_acceleration = MAX_ACC;
grabTask.player.max_deceleration = MAX_ACC;
grabTask.player.max_speed = MAX_VEL;
grabTask.player.vel = Utils::Polar2Vector(50 * 10, (move_Point - me.Pos()).dir());
// 初始速度前馈,加快响应
grabTask.player.max_rot_acceleration = MAX_ROT_ACC;
grabTask.player.max_rot_speed = MAX_ROT_SPEED;
grabTask.player.path_plan_in_circle = true;
grabTask.player.path_plan_circle_center = dribblePoint;
grabTask.player.path_plan_circle_radius = 200 * 10;
// 这使底层路径规划器在以 dribblePoint 为圆心、2000mm 为半径的圆形约束内规划路径,
// 确保机器人持球移动时球始终留在吸球装置范围内,不会因转弯过急而丢球。
setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(grabTask));
DribbleStatus::Instance()->setDribbleCommand(vecNumber, 3);
// 吸球,绕不死对面,对面绕晕了球就归我们了
_lastCycle = pVision->getCycle();
CStatedTask::plan(pVision);
}运动目标生成与子任务下发(第132-174行)
这段代码是 CZBreak::plan() 的核心执行部分,负责将决策结果转化为实际运动指令,分为三个阶段:
第一阶段:计算运动目标点(第143行)
move_Point = calc_point(pVision, vecNumber, passTarget, dribblePoint, grabMode);根据 grabMode 决定机器人本帧应移动到哪里:
| grabMode | move_Point 含义 |
|---|---|
ANTI_ROT | 原地不动(me.RawPos()),只旋转朝向 |
SIDE_MOVE | 圆弧上的下一个目标点(绕圆前进) |
同时 calc_point 还会更新全局 global_angle(目标朝向角)。
第二阶段:构建任务参数(第146-165行)
grabTask.player.pos = move_Point; // 目标位置
grabTask.player.angle = global_angle; // 目标朝向运动约束参数:
| 参数 | 值 | 含义 |
|---|---|---|
flag | NOT_AVOID_OUR|THEIR_VEHICLE | 不绕开任何机器人,强行突破 |
max_acceleration/deceleration | MAX_ACC | 加减速上限 |
max_speed | MAX_VEL | 最大速度 |
vel | Polar2Vector(500, 目标方向) | 初始速度前馈,加快响应 |
圆形路径规划(关键):
grabTask.player.path_plan_in_circle = true;
grabTask.player.path_plan_circle_center = dribblePoint; // 圆心 = 吸球口位置
grabTask.player.path_plan_circle_radius = 200 * 10; // 半径 = 2000mm这使底层路径规划器在以 dribblePoint 为圆心、2000mm 为半径的圆形约束内规划路径,确保机器人持球移动时球始终留在吸球装置范围内,不会因转弯过急而丢球。
第三阶段:下发任务并收尾(第167-173行)
setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(grabTask));
// 将构建好的任务交给底层运动控制器执行
DribbleStatus::Instance()->setDribbleCommand(vecNumber, 3);
// 保持吸球器工作,防止球在运动中脱落(功率固定为 3)
_lastCycle = pVision->getCycle();
// 记录本帧时间,供下帧超时检测使用整体数据流
judge_state() 决策
↓ grabMode
calc_point() 计算位置/朝向
↓ move_Point + global_angle
构建 grabTask(含圆形路径约束)
↓
SmartGotoPosition() 底层执行
+ 持续吸球calc_point
/**
* @name: calc_point
* @description: 根据当前 grabmode 计算机器人本帧应移动到的目标点,同时更新全局朝向角 global_angle
* @return {*}
*/
CGeoPoint calc_point(const CVisionModule *pVision, const int vecNumber, CGeoPoint &target,
const CGeoPoint &dribblePoint, int &grabmode)
{
const PlayerVisionT &me = pVision->ourPlayer(vecNumber);
const MobileVisionT &ball = pVision->ball();
CGeoPoint move_point = me.RawPos();
bool side_pass = me.RawPos().dist(final_Pos) < 120;
// 判断机器人是否已绕到 final_Pos(judge_state 中计算的绕圆落脚点)附近 120mm 内,用于触发终止绕圆、切换到射门/传球。
std::vector<CGeoPoint> enemy_points;
// 收集附近障碍敌人
for (int i = 0; i < PARAM::Field::MAX_PLAYER; i++)
{
auto test_enemy = pVision->theirPlayer(i);
if (test_enemy.Valid() &&
(test_enemy.Pos() - dribblePoint).mod() < CHECK_OBSTCLE_DIST &&
!Utils::InTheirPenaltyArea(test_enemy.Pos(), 0))
enemy_points.push_back(test_enemy.Pos());
}
switch (grabmode)
{
case SIDE_MOVE:
{// 绕圆
CVector cycle2me = me.RawPos() - cycleCenter;
double roll_Dir = cycle2me.dir() + rot_d * PARAM::Math::PI * 2 / (4.5 * 3);
move_point = cycleCenter + Utils::Polar2Vector(radio, roll_Dir);
// 每帧旋转约 26.7°,rot_d 为 ±1 控制顺/逆时针方向
// 机器人沿以 cycleCenter 为圆心、radio 为半径的圆弧逐帧移动绕过敌人。
GDebugEngine::Instance()->gui_debug_arc(cycleCenter, radio, 0, 360);
global_angle = Utils::Normalize(cycle2me.dir());
if (side_pass)
{// 到达终点时
global_angle = (target - me.RawPos()).dir();
// 朝向目标,准备射门/传球
move_point = me.RawPos();
// 停止移动,原地瞄准
}
break;
}
default:
{// ANTI_ROT 及默认分支
move_point = me.RawPos();
global_angle = rot_dir(pVision, vecNumber, target);
// rot_dir() 根据敌人位置决定旋转方向:
// 若敌人贴近且在我与目标之间,则朝敌人背后方向旋转,用身体把球护住转过去;
// 否则直接朝目标方向旋转。
}
}
return move_point;
}calc_point 详细解析
总体作用: 根据当前 grabmode 计算机器人本帧应移动到的目标点,同时更新全局朝向角 global_angle。
前置准备(第578-588行)
CGeoPoint move_point = me.RawPos(); // 默认目标点 = 当前位置(原地)
bool side_pass = me.RawPos().dist(final_Pos) < 120; // 是否已到达绕圆终点side_pass 判断机器人是否已绕到 final_Pos(judge_state 中计算的绕圆落脚点)附近 120mm 内,用于触发终止绕圆、切换到射门/传球。
同时收集附近障碍敌人(enemy_points),供后续扩展使用(当前 switch 中未直接用到)。
SIDE_MOVE 分支(第591-605行)
CVector cycle2me = me.RawPos() - cycleCenter; // 圆心→我 的向量
double roll_Dir = cycle2me.dir() + rot_d * PI*2/(4.5*3); // 在圆上前进一步的角度
move_point = cycleCenter + Polar2Vector(radio, roll_Dir); // 新目标点在圆弧上
global_angle = Normalize(cycle2me.dir()); // 朝向切线方向(沿圆周运动)圆弧步进原理:
θnext=θcurrent+rot_d⋅4.5×32π
每帧旋转约 13.52π≈26.7°,rot_d 为 ±1 控制顺/逆时针方向,机器人沿以 cycleCenter 为圆心、radio 为半径的圆弧逐帧移动绕过敌人。
到达终点时(side_pass == true):
global_angle = (target - me.RawPos()).dir(); // 朝向目标,准备射门/传球
move_point = me.RawPos(); // 停止移动,原地瞄准ANTI_ROT 及默认分支(第606-610行)
move_point = me.RawPos(); // 原地不动
global_angle = rot_dir(pVision, vecNumber, target); // 计算旋转朝向rot_dir() 根据敌人位置决定旋转方向:若敌人贴近且在我与目标之间,则朝敌人背后方向旋转,用身体把球护住转过去;否则直接朝目标方向旋转。
整体逻辑图
calc_point()
│
├─ SIDE_MOVE
│ ├─ 未到终点 → 圆弧上前进一步(持球绕圆)
│ └─ 已到终点(side_pass)→ 原地停,朝向目标
│
└─ ANTI_ROT(默认)
└─ 原地不动,调用 rot_dir() 计算护球旋转朝向关键设计:move_point 控制去哪,global_angle 控制朝哪转,两者分离,使运动和朝向可以独立优化。
贡献者
更新日志
f1210-ZBreak于