diff --git a/Core/src/Strategy/skill/GotoPosition.cpp b/Core/src/Strategy/skill/GotoPosition.cpp index 6f9dfe5..f6666d0 100644 --- a/Core/src/Strategy/skill/GotoPosition.cpp +++ b/Core/src/Strategy/skill/GotoPosition.cpp @@ -151,9 +151,6 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision) bool ignoreNotStop = false; target = avoidPenaltyArea(pVision, vecPos, target, avoidLength, vecNumber); - // 记录当前的规划执行目标点 - GDebugEngine::Instance()->gui_debug_x(target, TASK_TARGET_COLOR); - GDebugEngine::Instance()->gui_debug_line(self.Pos(), target, TASK_TARGET_COLOR); if(task().player.vel.mod() > 1e-8) { GDebugEngine::Instance()->gui_debug_line(target, target + task().player.vel / 10, COLOR_WHITE); } @@ -185,6 +182,12 @@ CPlayerCommand* CGotoPositionV2::execute(const CVisionModule* pVision) final.SetDir((playerFlag & (PlayerStatus::TURN_AROUND_FRONT)) ? self.Dir() : task().player.angle); final.SetVel(task().player.vel); final.SetRotVel(task().player.rotvel); + + // 记录当前的规划执行目标点 + GDebugEngine::Instance()->gui_debug_x(final.Pos(), TASK_TARGET_COLOR, 0, 20); + GDebugEngine::Instance()->gui_debug_line(target, target+Utils::Polar2Vector(100,final.Dir()), TASK_TARGET_COLOR); + GDebugEngine::Instance()->gui_debug_line(self.Pos(), target, TASK_TARGET_COLOR); + /// 调用控制方法 CControlModel control; float usedtime = target.dist(self.Pos()) / capability.maxSpeed / 1.414; // 单位:秒 diff --git a/Core/src/Utils/GDebugEngine.cpp b/Core/src/Utils/GDebugEngine.cpp index a6daf65..4f60fad 100644 --- a/Core/src/Utils/GDebugEngine.cpp +++ b/Core/src/Utils/GDebugEngine.cpp @@ -16,9 +16,9 @@ CGDebugEngine::CGDebugEngine(){ CGDebugEngine::~CGDebugEngine(){ } -void CGDebugEngine::gui_debug_x(const CGeoPoint& p, int debug_color,int RGB_value){ +void CGDebugEngine::gui_debug_x(const CGeoPoint& p, int debug_color,int RGB_value, const int size){ CGeoPoint basePos = p; - static const int x_line_length = 30; + const int x_line_length = size/2; const CGeoPoint p1 = basePos + CVector( x_line_length, x_line_length); const CGeoPoint p2 = basePos + CVector(-x_line_length,-x_line_length); gui_debug_line(p1, p2, debug_color,RGB_value); diff --git a/Core/src/Utils/GDebugEngine.h b/Core/src/Utils/GDebugEngine.h index 5b4bedf..306dd26 100644 --- a/Core/src/Utils/GDebugEngine.h +++ b/Core/src/Utils/GDebugEngine.h @@ -31,7 +31,7 @@ class CGDebugEngine{ public: CGDebugEngine(); ~CGDebugEngine(); - void gui_debug_x(const CGeoPoint& p, int debug_color = 1,int RGB_value=0); + void gui_debug_x(const CGeoPoint& p, int debug_color = 1,int RGB_value=0, const int size = 60 /*mm*/); void gui_debug_points(const std::vector points, int debug_color = 1,int RGB_value=0); void gui_debug_line(const CGeoPoint& p1, const CGeoPoint& p2, int debug_color = 1,int RGB_value=0); void gui_debug_arc(const CGeoPoint& p, double r, double start_angle, double span_angle, int debug_color = 1, int RGB_value=0);