[Core] add predict for obs

Robocup-ssl-China · Apr 17, 2024 · 78ce7b9 · 78ce7b9
1 parent de6b432
commit 78ce7b9
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 16 deletions.
diff --git a/Core/src/MotionControl/CMmotion.cpp b/Core/src/MotionControl/CMmotion.cpp
@@ -394,7 +394,8 @@ void goto_point_omni( const PlayerPoseT& start,
     double ang_a, factor_a;
     double time_a, time_a_acc, time_a_dec, time_a_flat, time;
     double time_acc, time_dec, time_flat;
-    compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time);
+    // compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time);
+    __new_compute_motion_2d(x, v, target_vel, max_accel, max_speed, accel_factor, a, time);
     // a = Utils::Polar2Vector(std::min(1.0,a.mod()),a.dir());
     factor_a = 1;
 

diff --git a/Core/src/OtherLibs/cmu/obstacle.cpp b/Core/src/OtherLibs/cmu/obstacle.cpp
@@ -35,7 +35,6 @@ namespace  {
     const double BALL_AVOID_DIST = PARAM::Field::BALL_SIZE + 5.0f;
     const float DEC_MAX = 450;
     const float ACCURATE_AVOID = 250;
-
 }
 //====================================================================//
 //    Obstacle class implementation
@@ -403,13 +402,17 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
         for(int i = 0; i < PARAM::Field::MAX_PLAYER; ++i) {
             const PlayerVisionT& teammate = pVision->ourPlayer(i);
             if((i != player) && teammate.Valid()) {
+                auto dist = teammateAvoidDist;
+                PARAM::Vehicle::V2::PLAYER_SIZE;
                 if(i == TaskMediator::Instance()->rightBack() ||
                         i == TaskMediator::Instance()->leftBack() ||
                         WorldModel::Instance()->CurrentRefereeMsg() == "GameStop" ||
-                        WorldModel::Instance()->CurrentRefereeMsg() == "OurTimeout")
-                    add_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), PARAM::Vehicle::V2::PLAYER_SIZE, 1, drawObs);
-                else
-                    add_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), teammateAvoidDist, 1, drawObs);
+                        WorldModel::Instance()->CurrentRefereeMsg() == "OurTimeout"){
+                    dist = PARAM::Vehicle::V2::PLAYER_SIZE;
+                }
+                auto move_dist = std::min(teammate.Vel().mod() * 0.5, 1000.0); // move for 0.5s
+                auto move_end = teammate.Pos() + Utils::Polar2Vector(move_dist, teammate.Vel().dir());
+                add_long_circle(vector2f(teammate.Pos().x(), teammate.Pos().y()), vector2f(move_end.x(),move_end.y()), vector2f(teammate.Vel().x(), teammate.Vel().y()), dist, 1, drawObs);
             }
         }
     }
@@ -422,9 +425,9 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
                 if((target.dist(opp.Pos()) < PARAM::Field::MAX_PLAYER_SIZE / 2) ) {
                     continue;
                 }
-                else {
-                    add_circle(vector2f(opp.Pos().x(), opp.Pos().y()), vector2f(opp.Vel().x(), opp.Vel().y()), oppAvoidDist, 1, drawObs);
-                }
+                auto move_dist = std::min(opp.Vel().mod() * 0.5, 1000.0); // move for 0.5s
+                auto move_end = opp.Pos() + Utils::Polar2Vector(move_dist, opp.Vel().dir());
+                add_long_circle(vector2f(opp.Pos().x(), opp.Pos().y()), vector2f(move_end.x(),move_end.y()), vector2f(opp.Vel().x(), opp.Vel().y()), oppAvoidDist, 1, drawObs);
             }
         }
     }

diff --git a/Core/tactics/skill/Touch.cpp b/Core/tactics/skill/Touch.cpp
@@ -42,21 +42,22 @@ void CTouch::plan(const CVisionModule* pVision){
     const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
 
     // stupid version of getballPos
-    CGeoPoint bestPos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
+    CGeoPoint bestMousePos = BallSpeedModel::instance()->poseForTime(1.0).Pos();
     for(double dist = 0; dist < 3000; dist += 20){
-        auto pos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
-        double t1 = predictedTime(me, pos);
+        auto _mousePos = ballPos + Utils::Polar2Vector(dist, ballVelDir);
+        auto _robot_pos = _mousePos+Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, ballVelDir);
+        double t1 = predictedTime(me, _robot_pos);
         double t2 = BallSpeedModel::Instance()->timeForDist(dist);
         if (DEBUG_SWITCH){
-            GDebugEngine::Instance()->gui_debug_x(pos,COLOR_GREEN);
-            GDebugEngine::Instance()->gui_debug_msg(pos, fmt::format("t:{:.1f}", t1-t2), COLOR_GREEN, 0, 30);
+            GDebugEngine::Instance()->gui_debug_x(_robot_pos,COLOR_GREEN, 0, 10);
+            GDebugEngine::Instance()->gui_debug_msg(_robot_pos+CVector(50,50), fmt::format("t:{:.1f},{:.1f}", t1, t2), COLOR_RED, 0, 10, 30);
         }
         if(t1 < t2 || t1 < 0.1){
-            bestPos = pos;
+            bestMousePos = _mousePos;
             break;
         }
     }
-    const auto getBallPos = bestPos; // TODO : replace with GetBallPos after skillutils
+    const auto getBallPos = bestMousePos; // TODO : replace with GetBallPos after skillutils
 
     const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
     const auto me2segTime = predictedTime(me, projectionRobotPos);