StaticGetBallV2
约 3068 字
初始化
void CStaticGetBallV2::plan(const CVisionModule *pVision){
if(pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE*0.1){
setState(BEGINNING);
_goBackball = true;
backTime = 0;
fraredOn = 0;
fraredOff = 0;
_flytimediff = 0;
_backjud = true;
_canKick = false;
_gopos = false;
thisNoUpdate = false;
noUpdatecnt=0;
readyTime = 0;
rot_flag = 1;
judge_once = true;
_judgeonce = false;
//realTarget = CGeoPoint(-99999,-99999);
} // 超时重置
int runner = task().executor;
CGeoPoint target = task().player.pos; // 传球的目标点
int flag = task().player.flag;
float kpower = task().player.kickpower; // 平射力度
float cpower = task().player.chipkickpower; // 挑射力度
double rotradio = BeckhamDecision::Instance()->rotradio();
// 机器人绕球时保持的距离半径,PARAM::Vehicle::V2::PLAYER_SIZE*2.5+PARAM::Field::BALL_SIZE/2;
bool is_blackCar = false;
ZSS::ZParamManager::instance()->loadParam(is_blackCar,QString("BlackCar/%1_car").arg(runner),false);
// 获取车型
const PlayerVisionT &me = pVision->ourPlayer(runner);
const MobileVisionT &ball = pVision->ball();
CGeoPoint ballPos = ball.Pos();//ball.Valid() ? ball.Pos() : ballLast;
CVector me2ball = ballPos - me.RawPos();
CVector me2target = target - me.RawPos();
CVector ball2target = target - ballPos;
CVector target2Ball = ballPos - target;
bool frared = Utils::isBallInFace(pVision,runner,PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,30);
// 综合视觉和红外,判断到底吸没吸到球
bool isflat = MessiDecision::Instance()->isfreeFlat();
// 问 Messi ,我该平射还是挑射,目前的状态是一直为平射,因为挑射射不准这段代码是静态取球技能的主规划函数入口,完成状态重置 + 任务参数提取 + 基础感知计算三件事。
1. 中断后状态重置
if(pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE*0.1)条件:当前帧与上次执行帧之差超过 10% 的帧率(即技能被中断超过约 1.5 帧),说明执行被打断,需要完全重置。
重置内容:
| 变量 | 含义 |
|---|---|
setState(BEGINNING) | 状态机回到初始状态 |
_goBackball = true | 强制机器人先退回到指定集结点 |
backTime / readyTime | 倒计时清零 |
fraredOn / fraredOff | 红外传感器计数清零 |
_flytimediff | 飞球时间差清零 |
_backjud / _canKick / _gopos | 各阶段判断标志位复位 |
rot_flag = 1 | 绕球方向重置为顺时针 |
judge_once = true | 允许重新判断绕球方向 |
2. 从任务中提取参数
int runner = task().executor; // 执行该技能的机器人编号
CGeoPoint target = task().player.pos; // 射门/传球目标点
int flag = task().player.flag; // 状态标志位
float kpower = task().player.kickpower; // 踢球力量
float cpower = task().player.chipkickpower; // 挑球力量
double rotradio = BeckhamDecision::Instance()->rotradio(); // 绕球半径rotradio 来自 BeckhamDecision 决策模块,决定机器人绕球时保持的距离半径。
3. 查询感知数据
const PlayerVisionT &me = pVision->ourPlayer(runner); // 己方机器人视觉数据
const MobileVisionT &ball = pVision->ball(); // 球的视觉数据
CGeoPoint ballPos = ball.Pos(); // 球的当前位置
CVector me2ball = ballPos - me.RawPos(); // 机器人→球 向量
CVector me2target = target - me.RawPos(); // 机器人→目标 向量
CVector ball2target = target - ballPos; // 球→目标 向量
CVector target2Ball = ballPos - target; // 目标→球 向量(反向)这四个向量是后续绕球路径规划的几何基础:
me2ball.dir()用于朝向控制ball2target.dir()用于确定踢球角度me2target.dir()用于行进方向
bool frared = Utils::isBallInFace(..., 30); // 红外传感器:球是否在机器人踢球面前方30mm内
bool isflat = MessiDecision::Instance()->isfreeFlat(); // 当前是否为平射模式frared 是后续判断是否可以执行踢球动作的关键硬件反馈,isflat 决定后续用平射还是挑射踢球。
goBackBall 跑到球后面
/************** set subtask **********************/
if (_goBackball)
{
// 第一阶段:先跑到球后面
CGeoPoint gotoPoint = BeckhamDecision::Instance()->get_leaderPos();
// 找 Beckham 要开球车开球的站位坐标
backTime = backTime > maxTime ? maxTime : backTime + 1;
// 已在开球点附近等待的帧数,上限 maxTime=1024,防止溢出
if ((me.Pos() - gotoPoint).mod() < 1.5 * 10 && BeckhamDecision::Instance()->get_kickBall())
{
// 机器人已到达集结点 且 Beckham 通知我可以踢球了
_goBackball = false;
}
GDebugEngine::Instance()->gui_debug_msg(ball.Pos() + CVector(0, 200), "goback");
if (ballPos.x() < 1000 && ballPos.x() > -1000 && ballPos.y() < 1000 && ballPos.y() > -1000)
{
// 球在场地中心区域(2m×2m范围内)
setSubTask(PlayerRole::makeItGoto(runner, gotoPoint, me2ball.dir(),
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
}
else
{
if (backTime > 200)
{
// 球在边缘(角球) 且 已等待超过200帧 → 可能是被对方的车卡住了,不避让对方车辆
setSubTask(PlayerRole::makeItGoto(
runner, gotoPoint, me2ball.dir(), PlayerStatus::NOT_AVOID_THEIR_VEHICLE,
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
}
else
{
// 球在边缘 但 等待时间短 → 正常跑点
setSubTask(
PlayerRole::makeItGoto(runner, gotoPoint, me2ball.dir(),
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
}
}
_judgeonce = false;
// 在goBack阶段持续清除"已判断绕球方向"的标志,确保进入取球阶段时重新判断绕球方向
}获取目标集结点 + 计时
CGeoPoint gotoPoint = BeckhamDecision::Instance()->get_leaderPos();
backTime = backTime > maxTime ? maxTime : backTime + 1;gotoPoint:由上层决策模块动态给出的集结坐标(通常是传球接应位置)backTime:已在集结点附近等待的帧数,上限maxTime=1024,防止溢出
退出条件:解除 _goBackball
if((me.Pos() - gotoPoint).mod() < 1.5 * 10 && BeckhamDecision::Instance()->get_kickBall()){
_goBackball = false;
}两个条件同时满足才能进入取球阶段:
| 条件 | 含义 |
|---|---|
(me.Pos() - gotoPoint).mod() < 15 | 机器人已到达集结点(误差 < 15mm) |
get_kickBall() == true | 上层决策发出"可以去踢球"的信号 |
调试输出
GDebugEngine::Instance()->gui_debug_msg(ball.Pos()+CVector(0,200),"goback");在球的位置上方 200mm 处显示 "goback" 字样,用于调试可视化。
根据球的位置选择导航策略
if(ballPos.x() < 1000 && ballPos.x() > -1000 && ballPos.y() < 1000 && ballPos.y()>-1000){
// 球在场地中心区域(2m×2m范围内)
setSubTask(PlayerRole::makeItGoto(runner, gotoPoint, me2ball.dir(),
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
} else {
if(backTime > 200){
// 球在边缘 且 已等待超过200帧 → 放弃避让对方车辆
setSubTask(PlayerRole::makeItGoto(runner, gotoPoint, me2ball.dir(),
PlayerStatus::NOT_AVOID_THEIR_VEHICLE,
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
} else {
// 球在边缘 但 等待时间短 → 正常导航
setSubTask(PlayerRole::makeItGoto(runner, gotoPoint, me2ball.dir(),
PlayerStatus::ALLOW_DSS | PlayerStatus::DODGE_BALL));
}
}三种情况的导航策略对比:
| 情况 | 球的位置 | backTime | 是否避让对方 |
|---|---|---|---|
| 情况1 | 场地中心区 | 任意 | 是(正常避障) |
| 情况2 | 场地边缘 | ≤200帧 | 是(正常避障) |
| 情况3 | 场地边缘 | >200帧 | 否(强行穿越) |
设计意图:球在边缘时,对方机器人可能阻挡路径。等待200帧(约3秒)后仍未到位,则启用
NOT_AVOID_THEIR_VEHICLE强行穿插,避免机器人无限绕路卡死。
所有情况都保留:
ALLOW_DSS:允许动态速度规划DODGE_BALL:绕开球,防止把球踢飞
重置绕球判断标志
_judgeonce = false;在退回阶段持续清除"已判断绕球方向"的标志,确保进入取球阶段时重新判断绕球方向。
绕球与踢球
else
{ // 绕球对准 + 冲球踢球
CGeoPoint rollPoint;
double rollDir;
if (judge_once)
{ // 只执行一次,判断机器人应该从哪个方向绕球:
if (Utils::Normalize((ballPos - target).dir() - (me.Pos() - ballPos).dir()) < 0)
{
rot_flag = -1;
}
else
{
rot_flag = 1;
}
judge_once = false;
}
rollDir = me2ball.dir() + PI + rot_flag * PI * 2 / (4.5 * 3);
// 每帧推进约 26.7° ,绕着球动,不触碰球。因为碰了球,就进入活球状态了
CGeoLine shootline(me.Pos(), me.Dir());
CGeoSegment balltargetLine(ballPos, target);
CGeoSegment me_to_target_line(me.Pos(), target);
CGeoPoint finalPos = ballPos + Utils::Polar2Vector(rotradio, ball2target.dir() + PI);
// 最终停留点:在球的后方(与目标相反方向),距离为 rotradio,即理想的击球准备位置
CGeoPoint projection = balltargetLine.projection(me.Pos());
double ball_to_target_proj = me_to_target_line.projection(ball.Pos()).dist(ball.Pos());
// 球到"机器人-目标连线"的垂直距离(对准程度)
double balll_to_shoot_proj = shootline.projection(ball.Pos()).dist(ball.Pos());
// 球到"机器人朝向射线"的垂直距离(朝向对准程度)
double temp_angle =
fabs(Utils::Normalize((finalPos - ball.Pos()).dir() - (me.Pos() - ball.Pos()).dir()));
//球员到球和最终点到球的差角
double temp_dist = (temp_angle / PARAM::Math::PI) * rotradio * 2;
// 角度差对应的弧长,用于决定绕球速度
if (_backjud) // 绕球阶段
{
CVector vel;
CVector me2rollPoint;
if (me.Pos().dist(finalPos) < 10 * 10 || _judgeonce)
{ // 已接近 finalPos(距离 < 100mm)或已触发 _judgeonce
_judgeonce = true;
readyTime = readyTime > maxTime ? maxTime : readyTime + 1;
if (is_blackCar)
{
if (readyTime > 25 && (ball_to_target_proj < 40 || balll_to_shoot_proj < 40) &&
shootline.projection(ball.Pos())
.dist(me_to_target_line.projection(ball.Pos())) < 40)
_backjud = false;
}
else
{
if (readyTime > 3 && (ball_to_target_proj < 30 || balll_to_shoot_proj < 30) &&
shootline.projection(ball.Pos())
.dist(me_to_target_line.projection(ball.Pos())) < 30)
_backjud = false;
}
// 机器人进入"精对准"等待,开始计时
// readyTime,并检查三重对准条件同时满足才解锁踢球:
// 条件 普通车 黑色车
// 等待帧数 readyTime > 3 readyTime > 25
// 球在目标连线附近 < 30mm < 40mm
// 两条瞄准线偏差 < 30mm < 40mm
// 满足条件 → _backjud = false,进入踢球阶段。
rollPoint = finalPos;
me2rollPoint = rollPoint - me.Pos();
if (is_blackCar) // 黑车要是给了前馈速度,直接冲出去了
vel = Utils::Polar2Vector(0, me2rollPoint.dir()); // CVector(0,0);
else
vel = Utils::Polar2Vector(50 * 10, me2rollPoint.dir());
}
else // 距离 finalPos 较远,继续绕球
{
rollPoint = ballPos + Utils::Polar2Vector(rotradio, rollDir);
me2rollPoint = rollPoint - me.Pos();
//绕球速度
if (is_blackCar)
{ // 黑车根据剩余弧长分档控速,因为黑车加速度太大了,不分档设置一下直接横冲直撞
if (temp_dist > 350) // CVector(0,0);
vel = Utils::Polar2Vector(300 * 10, me2rollPoint.dir()); //最高速度
else if (temp_dist > 250)
vel = Utils::Polar2Vector(200 * 10, me2rollPoint.dir()); //中档速度
else if (temp_dist > 100)
vel = Utils::Polar2Vector(100 * 10, me2rollPoint.dir()); //中档速度
else
vel = Utils::Polar2Vector(50 * 10, me2rollPoint.dir()); //低档速度
}
else
vel = Utils::Polar2Vector(100 * 10, me2rollPoint.dir());
}
setSubTask(PlayerRole::makeItGoto(runner, rollPoint, me2target.dir(), vel, 0.0,
PlayerStatus::ALLOW_DSS));
}
else
{ // 踢球阶段
double taskdir;
CVector vel;
CGeoPoint gotoPoint;
taskdir = me2target.dir();
gotoPoint =
ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER - 70,
me2target.dir() + PI);
// 目标点在球的正后方偏 70mm 处(比完整对球距离少 70mm),让机器人主动冲入球中完成击球。
// if(frared) gotoPoint = me.RawPos();
if (fabs(Utils::Normalize(me.Dir() - me2target.dir())) < 60.0 * PI / 180.0)
{ // 机器人朝向与目标方向偏差 < 60° 才允许 _canKick = true。
_canKick = true;
}
else
{
_canKick = false;
}
if (is_blackCar) // 黑车限限速,不然又是大力
vel = Utils::Polar2Vector(200 * 10, me2target.dir());
else
vel = Utils::Polar2Vector(300 * 10, me2target.dir());
TaskT kick_task(task());
kick_task.player.pos = gotoPoint;
kick_task.player.angle = taskdir;
kick_task.executor = runner;
kick_task.player.vel = vel;
kick_task.player.flag = PlayerStatus::ALLOW_DSS;
kick_task.player.rotate_acc = 15; // 给个旋转加速度的限制,黑车转太快球吸不住的
// setSubTask(PlayerRole::makeItGoto(runner,gotoPoint,taskdir,vel,0.0,PlayerStatus::ALLOW_DSS));
setSubTask(TaskFactoryV2::Instance()->SmartGotoposition_Black(kick_task));
}
}
if (BeckhamDecision::Instance()->judgePassMode() == PASSMODE::FLAT)
{
if (_canKick)
KickStatus::Instance()->setKick(runner, kpower); // 能踢了就设置
}else 分支解析:第二阶段——绕球对准 + 冲球踢球(第120~232行)
这段代码是整个技能的核心,实现了机器人从侧面绕到球后方、对准目标、最终完成踢球的完整状态机。
一、一次性判断绕球方向(judge_once)
if(Utils::Normalize((ballPos - target).dir() - (me.Pos() - ballPos).dir()) < 0)
rot_flag = -1; // 逆时针绕
else
rot_flag = 1; // 顺时针绕只执行一次,判断机器人应该从哪个方向绕球:
Δθ=dir(ball→target)−dir(me→ball)
- Δθ<0:机器人在目标连线左侧 → 逆时针绕(
rot_flag = -1) - Δθ≥0:机器人在目标连线右侧 → 顺时针绕(
rot_flag = 1)
目的:选最短路径绕球,避免绕远路。
二、核心几何量计算
rollDir = me2ball.dir() + PI + rot_flag * PI*2/(4.5*3);me2ball.dir() + PI:从球出发、背离机器人的方向+ rot_flag * 26.7°:在此基础上沿绕球方向偏转一步(每帧推进约 26.7°)- 最终得到下一个绕球路径点的角度
CGeoPoint finalPos = ballPos + Polar2Vector(rotradio, ball2target.dir() + PI);最终停留点:在球的后方(与目标相反方向),距离为 rotradio,即理想的击球准备位置。
target
↑
|
ball
|
[finalPos] ← 机器人最终要到达的点
(me)| 变量 | 含义 |
|---|---|
ball_to_target_proj | 球到"机器人-目标连线"的垂直距离(对准程度) |
balll_to_shoot_proj | 球到"机器人朝向射线"的垂直距离(朝向对准程度) |
temp_angle | 机器人当前位置 vs 最终位置,相对于球的角度差 |
temp_dist | 角度差对应的弧长,用于决定绕球速度 |
三、_backjud == true:绕球阶段
子阶段1:已接近 finalPos(距离 < 100mm)或已触发 _judgeonce
if(me.Pos().dist(finalPos) < 10*10 || _judgeonce)机器人进入"精对准"等待,开始计时 readyTime,并检查三重对准条件同时满足才解锁踢球:
| 条件 | 普通车 | 黑色车 |
|---|---|---|
| 等待帧数 | readyTime > 3 | readyTime > 25 |
| 球在目标连线附近 | < 30mm | < 40mm |
| 两条瞄准线偏差 | < 30mm | < 40mm |
满足条件 → _backjud = false,进入踢球阶段。
速度控制:黑色车静止等待(vel = 0),普通车缓慢靠近(500 mm/s)。
子阶段2:距离 finalPos 较远,继续绕球
rollPoint = ballPos + Polar2Vector(rotradio, rollDir);按 rollDir 角度计算圆弧上的下一个目标点,然后根据剩余弧长分档控速(仅黑色车):
剩余弧长 temp_dist | 速度 |
|---|---|
| > 350mm | 3000 mm/s(最高) |
| > 250mm | 2000 mm/s |
| > 100mm | 1000 mm/s |
| ≤ 100mm | 500 mm/s(低速精准) |
普通车固定 1000 mm/s。
四、_backjud == false:冲球踢球阶段
gotoPoint = ballPos + Polar2Vector(PLAYER_CENTER_TO_BALL_CENTER - 70, me2target.dir() + PI);目标点在球的正后方偏 70mm 处(比完整对球距离少 70mm),让机器人主动冲入球中完成击球。
踢球使能判断:
if(fabs(Normalize(me.Dir() - me2target.dir())) < 60° )
_canKick = true;机器人朝向与目标方向偏差 < 60° 才允许 _canKick = true。
速度:黑色车 2000 mm/s,普通车 3000 mm/s。
使用 SmartGotoposition_Black 代替简单的 makeItGoto,实现更精确的位姿控制(含旋转加速度 rotate_acc=15)。
整体状态机总结
_goBackball=true → 跑到集结点
↓
_backjud=true, 绕球中 → 沿圆弧逼近 finalPos
↓
_backjud=true, 精对准 → 等待3/25帧 且 三轴对准
↓
_backjud=false → 冲球,_canKick=true 时发出踢球指令贡献者
更新日志
66dbc-chipsolver StaticGeiBallV2 WBack于