Skip to content

Commit

Permalink
[Client & Core] cameramatrix autoupdate & update touch
Browse files Browse the repository at this point in the history
  • Loading branch information
@Mark-tz
Mark-tz committed Apr 10, 2024
1 parent 4d43c23 commit 8ec1d12
Show file tree
Hide file tree
Showing 9 changed files with 84 additions and 66 deletions.
3 changes: 3 additions & 0 deletions Client/plugins/sim/configwidget.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "configwidget.h"
namespace{
auto ppm = ZSS::pm::instance();
auto zpm = ZSS::ZParamManager::instance();
}
//#define END_ENUM(parents, name) \
// parents->addChild(v_##name);
Expand All @@ -15,6 +16,8 @@ namespace{
#define ADD_VALUE(parent,type,name,defaultvalue,namestring) \
ppm->loadParam(v_##name,#parent"/"#name,defaultvalue);

#define ADD_CLIENT_VALUE(parent, type, name, defaultvalue, namestring) \
zpm->loadParam(v_##name, #parent "/" #name, defaultvalue);

ConfigWidget::ConfigWidget()
{
Expand Down
68 changes: 27 additions & 41 deletions Client/src/globaldata.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,48 +98,19 @@ void CGlobalData::saoConvertEdge() {
void CGlobalData::setCameraMatrix(bool real) {
zpm->loadParam(saoAction, "Alert/SaoAction", 0);
if (real) {
/*
//NEED CHANGE IN CANADA
for (int i = 0; i < PARAM::CAMERA; i++) {
double x, y;
vpm->loadParam(x, "Camera" + QString::number(i) + "CenterX", 0);
vpm->loadParam(y, "Camera" + QString::number(i) + "CenterY", 0);
cameraMatrix[i].fillCenter(saoConvert(CGeoPoint(x, y)));
}
cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(2410,1579)));//(-4500,2250);
cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(2162,-1529)));//(-1500,2250);
cameraMatrix[2].fillCenter(saoConvert(CGeoPoint(-2195,-1404)));//(1500,2250);
cameraMatrix[3].fillCenter(saoConvert(CGeoPoint(-2310,1477)));//(4500,2250);
// cameraMatrix[4].fillCenter(saoConvert(CGeoPoint(-4500,-2250)));
// cameraMatrix[5].fillCenter(saoConvert(CGeoPoint(-1500,-2250)));
// cameraMatrix[6].fillCenter(saoConvert(CGeoPoint(1500,-2250)));
// cameraMatrix[7].fillCenter(saoConvert(CGeoPoint(4500,-2250)));
for (int i = 0; i < PARAM::CAMERA; i++) {
vpm->loadParam(cameraMatrix[i].leftedge.min, "Camera" + QString::number(i) + "Leftmin", cameraMatrix[i].campos.x());
vpm->loadParam(cameraMatrix[i].leftedge.max, "Camera" + QString::number(i) + "Leftmax", cameraMatrix[i].campos.x());
vpm->loadParam(cameraMatrix[i].rightedge.min, "Camera" + QString::number(i) + "Rightmin", cameraMatrix[i].campos.x());
vpm->loadParam(cameraMatrix[i].rightedge.max, "Camera" + QString::number(i) + "Rightmax", cameraMatrix[i].campos.x());
vpm->loadParam(cameraMatrix[i].upedge.min, "Camera" + QString::number(i) + "Upmin", cameraMatrix[i].campos.y());
vpm->loadParam(cameraMatrix[i].upedge.max, "Camera" + QString::number(i) + "Upmax", cameraMatrix[i].campos.y());
vpm->loadParam(cameraMatrix[i].downedge.min, "Camera" + QString::number(i) + "Downmin", cameraMatrix[i].campos.y());
vpm->loadParam(cameraMatrix[i].downedge.max, "Camera" + QString::number(i) + "Downmax", cameraMatrix[i].campos.y());
}
saoConvertEdge();
*/
cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(-3170, 0)));
cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(3000, 0))); //(-4500,2250);

for (int i = 0; i < PARAM::CAMERA; i = i + 1) {
cameraMatrix[i].leftedge.min = cameraMatrix[i].campos.x() - 2800;
cameraMatrix[i].leftedge.max = cameraMatrix[i].campos.x() - 3400;
cameraMatrix[i].rightedge.min = cameraMatrix[i].campos.x() + 2800;
cameraMatrix[i].rightedge.max = cameraMatrix[i].campos.x() + 3400;
cameraMatrix[i].downedge.min = cameraMatrix[i].campos.y() - 4800;
cameraMatrix[i].downedge.max = cameraMatrix[i].campos.y() - 4800;
cameraMatrix[i].upedge.min = cameraMatrix[i].campos.y() + 4900;
cameraMatrix[i].upedge.max = cameraMatrix[i].campos.y() + 4900;
for (int i = 0; i < PARAM::CAMERA; i++)
{
cameraMatrix[i].fillCenter((CGeoPoint(0.0, 0.0)));
cameraMatrix[i].leftedge.min = 0;
cameraMatrix[i].leftedge.max = 0;
cameraMatrix[i].rightedge.min = 0;
cameraMatrix[i].rightedge.max = 0;
cameraMatrix[i].downedge.min = 0;
cameraMatrix[i].downedge.max = 0;
cameraMatrix[i].upedge.min = 0;
cameraMatrix[i].upedge.max = 0;
}
CameraInit();
} else { //for Sim
cameraMatrix[0].fillCenter(saoConvert(CGeoPoint(2410, 1579))); //(-4500,2250);
cameraMatrix[1].fillCenter(saoConvert(CGeoPoint(2162, -1529))); //(-1500,2250);
Expand Down Expand Up @@ -167,3 +138,18 @@ void CGlobalData::setCameraMatrix(bool real) {
}
}
}

void CGlobalData::CameraInit()
{
for (int i = 0; i < PARAM::CAMERA; i++)
{
cameraMatrix[i].leftedge.min = 0;
cameraMatrix[i].leftedge.max = 0;
cameraMatrix[i].rightedge.min = 0;
cameraMatrix[i].rightedge.max = 0;
cameraMatrix[i].downedge.min = 0;
cameraMatrix[i].downedge.max = 0;
cameraMatrix[i].upedge.min = 0;
cameraMatrix[i].upedge.max = 0;
}
}
4 changes: 3 additions & 1 deletion Client/src/globaldata.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,9 @@ class CGlobalData {
bool ctrlC;
QMutex ctrlCMutex;

private:
void CameraInit();

private:
CGeoPoint saoConvert(CGeoPoint);
void saoConvertEdge();
int saoAction;
Expand Down
2 changes: 1 addition & 1 deletion Client/src/vision/visionmodule.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ void CVisionModule::readRemoteSimData(){
*/
bool CVisionModule::dealWithData() {
counter++;
if (IF_EDGE_TEST) edgeTest();
edgeTest();
DealBall::instance()->run();
DealRobot::instance()->run();
Maintain::instance()->run();
Expand Down
2 changes: 1 addition & 1 deletion Core/src/Algorithm/BallSpeedModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
this->update();
auto maxDist = _ballVel.mod() * _ballVel.mod() / (2 * _DEC);
if (dist > maxDist) {
return {-1, CVector(0,0)};
return {99999, CVector(0,0)};
}
auto v0 = _ballVel.mod();
auto v1 = sqrt(v0 * v0 - 2 * _DEC * dist);
Expand Down
2 changes: 1 addition & 1 deletion Core/src/Strategy/skill/SmartGotoPosition.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,7 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)

obstacles obs(avoidLength);
if(!(playerFlag & PlayerStatus::BREAK_THROUGH))
obs.addObs(pVision, task(), DRAW_OBSTACLES, OPP_AVOID_DIST, TEAMMATE_AVOID_DIST, BALL_AVOID_DIST);
obs.addObs(pVision, task(), DRAW_OBSTACLES, OPP_AVOID_DIST, TEAMMATE_AVOID_DIST, std::max(task().ball.avoid_dist, BALL_AVOID_DIST));
else
obs.addObs(pVision, task(), DRAW_OBSTACLES, PARAM::Vehicle::V2::PLAYER_SIZE + PARAM::Field::BALL_SIZE + 20, PARAM::Vehicle::V2::PLAYER_SIZE + PARAM::Field::BALL_SIZE + 20.0, PARAM::Field::BALL_SIZE);

Expand Down
1 change: 1 addition & 0 deletions Core/src/Utils/misc_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,7 @@ struct stBallStatus{
int Sender; // 出球者号码（added by shizhy)
double angle;
bool front;
double avoid_dist = -1;
};

/// 任务结构
Expand Down
2 changes: 1 addition & 1 deletion Core/tactics/play/TestGetBall.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ return {
local dist = (ball.pos() - player.pos("Assister")):mod()
debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
end,
Assister = task.touchKick(CGeoPoint(6000,0),nil,1000,true),
Assister = task.touchKick(CGeoPoint(6000,0),nil,6000,true),
-- Assister = task.stop(),
match = "[A]"
},
Expand Down
66 changes: 46 additions & 20 deletions Core/tactics/skill/Touch.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,10 @@
namespace{
const CGeoPoint THEIR_GOAL = CGeoPoint(PARAM::Field::PITCH_LENGTH/2,0);
bool DEBUG_SWITCH;
template<typename T>
T clip(T x, T min, T max){
return std::max(min, std::min(max, x));
}
}
CTouch::CTouch(){
ZSS::ZParamManager::instance()->loadParam(DEBUG_SWITCH,"Debug/Touch",false);
Expand All @@ -36,47 +40,69 @@ void CTouch::plan(const CVisionModule* pVision){
const double toBallDist = mouse2Ball.mod();
// predict ball pos
const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
const auto getBallPos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); // TODO : replace with GetBallPos after skillutils


// stupid version of getballPos
CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
for(double dist = 0; dist < 3000; dist += 100){
auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
double t1 = predictedTimeWithRawVel(me, pos);
double t2 = BallSpeedModel::Instance()->timeForDist(dist);
GDebugEngine::Instance()->gui_debug_x(pos,COLOR_GREEN);
GDebugEngine::Instance()->gui_debug_msg(pos, fmt::format("t:{:.2f},{:.2f}", t1, t2), COLOR_GREEN);
if(t1 < t2 || t1 < 0.1){
bestPos = pos;
break;
}
}
const auto getBallPos = bestPos; // TODO : replace with GetBallPos after skillutils

const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
const auto me2segTime = predictedTimeWithRawVel(me, projectionRobotPos);
const auto ball2segTime = BallSpeedModel::Instance()->timeForDist(ballPos.dist(projectionMousePos));
const bool canWaitForBall = ballRunningSeg.IsPointOnLineOnSegment(projectionMousePos) && me2segTime < ball2segTime;
const auto predictPos = ballVelMod > 100 ? getBallPos : ballPos;
const auto predictPos = ballVelMod > 300 ? getBallPos : ballPos;
const double ballVel_ball2Target_ad = ballVelMod > 300 ? angleDiff(ballVelDir, (target - ballPos).dir()) : 180;
const bool angleCanTouch = std::abs(ballVel_ball2Target_ad) > 100 / 180.0 * PARAM::Math::PI;

CGeoPoint targetRunPos;
double targetRunDir;
CVector targetRunVel;
if (!canWaitForBall){
targetRunDir = (target - predictPos).dir();
targetRunPos = predictPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER - 5, targetRunDir + PARAM::Math::PI);
targetRunVel = Utils::Polar2Vector(300, mouse2Ball.dir());
}
else{
targetRunDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
targetRunPos = projectionMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER-5,targetRunDir + PARAM::Math::PI);
}
const CVector targetRunVel = canWaitForBall ? CVector(0, 0) : Utils::Polar2Vector(200, ballVelDir);
const CGeoPoint targetMousePos = canWaitForBall ? projectionMousePos : predictPos;
const double targetRunDir = (useInter || !angleCanTouch) ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - targetMousePos).dir();
const CGeoPoint targetRunPos = targetMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, targetRunDir + PARAM::Math::PI);

// add avoid ball flag
const auto me2target = targetRunPos - me.Pos();
const auto me2TargetSeg = CGeoSegment(me.Pos(), targetRunPos);
const auto runTarget2kickTarget = target - targetRunPos;
// add avoid ball flag
auto angle_diff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-800,-800),fmt::format("anglediff {:.2f}",angle_diff/PARAM::Math::PI*180.0));
if (toBallDist > 150 && std::abs(angle_diff) > (100.0) / 180.0 * PARAM::Math::PI)
{

const auto BALL_STATIC = ballVelMod < 300;
const auto runTargetDiff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
const auto needAvoidStatic = BALL_STATIC && runTargetDiff;
const auto avoidDistStatic = needAvoidStatic ? 30.0 : 0.0;

const auto diff4avoid_ball = std::abs(angleDiff(me2target.dir(), (targetMousePos - ballPos).dir()));
const auto needAvoidDynamic = !BALL_STATIC && me2TargetSeg.IsPointOnLineOnSegment(ballPos) && diff4avoid_ball < 90 / 180.0 * PARAM::Math::PI;
const auto avoidDistDynamic = needAvoidDynamic ? 3*clip(90-diff4avoid_ball *180.0 / PARAM::Math::PI,0.0,90.0) : 0;

double avoid_dist = 0;
if (toBallDist > 120 && (needAvoidStatic || needAvoidDynamic)){
taskFlag |= PlayerStatus::DODGE_BALL;
avoid_dist = std::max(avoidDistStatic, avoidDistDynamic);
}

TaskT newTask(task());
newTask.player.pos = targetRunPos;
newTask.player.angle = targetRunDir;
newTask.player.vel = targetRunVel;
newTask.player.flag = taskFlag;
newTask.ball.avoid_dist = 300;
setSubTask("SmartGoto", newTask);

if(DEBUG_SWITCH){
auto endPos = ballPos + Utils::Polar2Vector(ballVelMod,ballVelDir);
GDebugEngine::Instance()->gui_debug_line(ballPos,endPos,4);
GDebugEngine::Instance()->gui_debug_msg(targetRunPos, fmt::format("TVel:{},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
GDebugEngine::Instance()->gui_debug_msg(targetRunPos, fmt::format("TVel:{:.0f},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
GDebugEngine::Instance()->gui_debug_msg(targetRunPos+CVector(0,120), fmt::format("modeDif:{:.1f}", ballVel_ball2Target_ad / PARAM::Math::PI * 180.0));
}

_lastCycle = pVision->getCycle();
Expand Down

0 comments on commit 8ec1d12

Please sign in to comment.