[Core] clear worlddefine & add rawpos

Core/src/Algorithm/BallSpeedModel.cpp

@@ -40,9 +40,9 @@ std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
 	auto time = (v0 - v1) / _DEC;
 	return {time, Utils::Polar2Vector(v1, _ballVel.dir())};
 }
-MobileVisionT CBallSpeedModel::poseForTime(const double time){
+ObjectPoseT CBallSpeedModel::poseForTime(const double time){
 	this->update();
-	MobileVisionT predictPose;
+	ObjectPoseT predictPose;
 	auto v0 = _ballVel.mod();
 	auto v1 = std::max(v0 - _DEC * time, 0.0);
 	auto runtime = std::min(v0 / _DEC, time);

Core/src/Algorithm/BallSpeedModel.h

@@ -14,7 +14,7 @@ class CBallSpeedModel
 	std::tuple<double,CVector> predictForDist(const double dist);	// 计算若干距离后的速度
 	double timeForDist(const double dist);      //计算球运动若干距离的时间
 	CVector speedForDist(const double dist);	// 计算球运动若干距离的速度
-	MobileVisionT poseForTime(const double time); // 计算若干帧以后的绝对位置
+	ObjectPoseT poseForTime(const double time); // 计算若干帧以后的绝对位置
 private:
 	void update();
 	CGeoPoint _ballPos;

Core/src/Algorithm/BallStatus.cpp

@@ -55,7 +55,7 @@ void CBallStatus::UpdateBallStatus(const CVisionModule* pVision)
 
 void CBallStatus::UpdateBallMoving(const CVisionModule* pVision)
 {
-	const MobileVisionT& ball = pVision->ball(); // 球	
+	const ObjectPoseT& ball = pVision->ball(); // 球	
 	isNearPlayer = false;
 	for (int i=0; i<PARAM::Field::MAX_PLAYER*2; i++){
 		if (pVision->allPlayer(i).Valid() && pVision->allPlayer(i).Pos().dist(ball.Pos())< PARAM::Field::MAX_PLAYER_SIZE/2+5){

Core/src/Algorithm/ContactChecker.cpp

@@ -33,7 +33,7 @@ bool isPointOutField(CGeoPoint pos)
 }
 void ContactChecker::OutFieldJudge(const CVisionModule* pVision)
 {
-	MobileVisionT last_ball=pVision->ball(pVision->getCycle()-4);
+	ObjectPoseT last_ball=pVision->ball(pVision->getCycle()-4);
 	if (pVision->ball().Valid()&&isPointOutField(pVision->ball().Pos())){
 		_isBallOutField=true;
 	}

Core/src/Algorithm/ContactChecker.h

@@ -23,8 +23,8 @@ class ContactChecker {
     std::vector<PlayerVisionT> AllPlayer;
     Last_Contact _last_contact;
 
-    MobileVisionT _ball;
-    MobileVisionT _lastball;
+    ObjectPoseT _ball;
+    ObjectPoseT _lastball;
     CVector ball_direction_before;
     CVector ball_direction_after;
     double speed_before ;

Core/src/LuaModule/ballspeedmodel.pkg

@@ -3,5 +3,5 @@ class CBallSpeedModel
 {
 	double timeForDist(const double dist);
 	CVector speedForDist(const double dist);
-	MobileVisionT poseForTime(const double time);
+	ObjectPoseT poseForTime(const double time);
 };

Core/src/LuaModule/visionmodule.pkg

@@ -17,10 +17,10 @@ class CVisionModule{
 	const PlayerVisionT& allPlayer(int num) const;
 	const PlayerVisionT& ourPlayer(int num) const;
 	const PlayerVisionT& theirPlayer(int num) const;
-	const MobileVisionT& ball() const;
+	const ObjectPoseT& ball() const;
 	const PlayerVisionT& ourPlayer(int cycle, int num) const;
 	const PlayerVisionT& theirPlayer(int cycle, int num) const;
-	const MobileVisionT& ball(int cycle) const;
+	const ObjectPoseT& ball(int cycle) const;
 	const ObjectPoseT& rawBall() const;
         const CGeoPoint& getBallPlacementPosition() const;
 	const RobotRawVisionData& ourRawPlayer(int num) const;

Core/src/LuaModule/worlddefine.pkg

@@ -16,18 +16,11 @@ class ObjectPoseT{
 	double VelY() const;
 	void SetValid(bool v);
 	bool Valid() const;
-};
-
-class VisionObjectT{
 	const CGeoPoint& RawPos() const;
 	void SetRawPos(double x, double y);
 	void SetRawPos(const CGeoPoint& pos);
 };
 
-class MobileVisionT : public ObjectPoseT, public VisionObjectT{
-
-};
-
 struct PlayerPoseT : public ObjectPoseT{
 	PlayerPoseT();
 	double Dir() const;
@@ -39,12 +32,12 @@ struct PlayerPoseT : public ObjectPoseT{
 };
 
 class PlayerTypeT{
-	void SetType(int t);
-	int Type() const;
-};
 
-class PlayerVisionT : public PlayerPoseT, public VisionObjectT, public PlayerTypeT{
+};
 
+class PlayerVisionT : public PlayerPoseT {
+	void SetType(int t);
+	int Type() const;
 };
 
 struct PlayerCapabilityT{

Core/src/MotionControl/DynamicsSafetySearch.cpp

@@ -383,7 +383,7 @@ bool CDynamicSafetySearch::CheckAccel (const int player, vector2f acc, const CVi
     if(pVision->ball().Valid() && (_flag & PlayerStatus::DODGE_BALL)) {
         vector2f pj = vector2f(pVision->ball().Pos().x(), pVision->ball().Pos().y());
         vector2f vj = vector2f(pVision->ball().VelX(), pVision->ball().VelY());
-        const MobileVisionT& last_ball = pVision->ball(pVision->getLastCycle());
+        const ObjectPoseT& last_ball = pVision->ball(pVision->getLastCycle());
         vector2f Aj;
         Aj = (vj - vector2f(last_ball.VelX(), last_ball.VelY())) /_C;
         if(CheckRobot(player, _pos, _vel, acc, -1, pj, vj, Aj, type, limitTime) == UNSAFE) {

Core/src/OtherLibs/cmu/obstacle.cpp

@@ -442,12 +442,12 @@ void obstacles::addObs(const CVisionModule *pVision, const TaskT &task, bool dra
 
     // ball
     if(flags & PlayerStatus::DODGE_BALL) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         add_circle(vector2f(ball.Pos().x(), ball.Pos().y()), vector2f(ball.Vel().x(), ball.Vel().y()), ballAvoidDist, 1, drawObs);
     }
 
     if(WorldModel::Instance()->CurrentRefereeMsg() == "GameStop") {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         add_circle(vector2f(ball.Pos().x(), ball.Pos().y()), vector2f(0.0f, 0.0f), 50.0f, 1, drawObs);
     }
 

Core/src/PathPlan/ObstacleNew.cpp

@@ -486,13 +486,13 @@ void ObstaclesNew::addObs(const CVisionModule * pVision, const TaskT & task, boo
 
     // ball
     if (flags & PlayerStatus::DODGE_BALL) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         addCircle(ball.RawPos(), ball.Vel(), ballAvoidDist, OBS_CIRCLE_NEW);
     }
 
     if (WorldModel::Instance()->CurrentRefereeMsg() == "GameStop" ||
             (flags & PlayerStatus::AVOID_STOP_BALL_CIRCLE)) {
-        const MobileVisionT& ball = pVision->ball();
+        const ObjectPoseT& ball = pVision->ball();
         addCircle(ball.RawPos(), CVector(0.0f, 0.0f), stopBallAvoidDist, OBS_CIRCLE_NEW);
     }
 

Core/src/Vision/BallPredictor.cpp

@@ -86,8 +86,8 @@ bool CBallPredictor::checkValid(int cycle) {
     return true;
 }
 
-void CBallPredictor::setCollisionResult(int cycle, const MobileVisionT& ball) {
-    MobileVisionT& oldBall = _visionLogger.getVision(cycle);
+void CBallPredictor::setCollisionResult(int cycle, const ObjectPoseT& ball) {
+    ObjectPoseT& oldBall = _visionLogger.getVision(cycle);
     oldBall = ball;
     _hasCollision = true;
 

Core/src/Vision/BallPredictor.h

@@ -21,7 +21,7 @@ const double MAX_DIST = 1000.0 / PARAM::Vision::FRAME_RATE * 4;
 const int MAX_LOGS = 16;
 }
 
-class BallVisionData : public MobileVisionT {
+class BallVisionData : public ObjectPoseT {
   public:
     BallVisionData(): cycle(-1) {}
     int cycle;
@@ -64,10 +64,10 @@ class CBallPredictor {
     BallVisionData& getData(int cycle) {
         return _visionLogger.getVision(cycle);
     }
-    const MobileVisionT& getResult(int cycle) const {
+    const ObjectPoseT& getResult(int cycle) const {
         return _visionLogger.getVision(cycle);
     }
-    void setCollisionResult(int cycle, const MobileVisionT& ball);
+    void setCollisionResult(int cycle, const ObjectPoseT& ball);
     int visibility() const {
         return _visibility;
     }
@@ -91,7 +91,7 @@ class CBallPredictor {
     bool checkValid(int cycle); // 去掉不合理的情况
   private:
     CBallVisionLogger _visionLogger;
-    MobileVisionT _ballLinePredictData[60];
+    ObjectPoseT _ballLinePredictData[60];
     int _ballLostTime; // 看不到球的次数
     int _ballInvalidMovedCycle; // 球的信息不合理的周期数
     int _visibility, _activity; // 可见度和活动度

Core/src/Vision/CollisionSimulator.h

@@ -9,10 +9,10 @@ class CCollisionSimulator{
 	CCollisionSimulator() : _hasCollision(false){ }
 	void reset(const CGeoPoint& ballRawPos, const CVector& ballVel);
 	void simulate(PlayerVisionT robot, const double time); // 模拟一定时间
-	const MobileVisionT& ball() const { return _ball; }
+	const ObjectPoseT& ball() const { return _ball; }
 	bool hasCollision() const { return _hasCollision; }
 private:
-	MobileVisionT _ball; // 球的信息
+	ObjectPoseT _ball; // 球的信息
 	CVector _ballRelToRobot; // 球在机器人局部坐标系中的位置
 	bool _hasCollision; // 是否碰撞
 };

Core/src/Vision/RobotPredictor.cpp

@@ -34,7 +34,7 @@ CRobotPredictor::CRobotPredictor(bool isHasRotation) : _robotLostTime(0), _isHas
 //        _robotFilter.initialize(PARAM::File::RobotPosFilterDir + "slowMatrices.txt", PARAM::File::RobotPosFilterDir + "fastMatrices.txt");
 //    }
 }
-void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert) {
+void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert) {
     // !!!!这里好像有问题:
     // 如果是对手车也能识别朝向,就会调用updateOurVision, 但因为在该函数里取不到cmd(只有我方车才有cmd存下来), 结果就不对.
     // 这里需要一个辨别我方车与对方车的方式!!
@@ -95,7 +95,7 @@ void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const
     }
 }
 
-//void CRobotPredictor::updateOurVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum) {
+//void CRobotPredictor::updateOurVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum) {
 //    const double x = invert ? -player.pos.x : player.pos.x;
 //    const double y = invert ? -player.pos.y : player.pos.y;
 //    double dir = 0;
@@ -279,7 +279,7 @@ void CRobotPredictor::updateVision(int cycle, const VehicleInfoT& player, const
 //    predictedVision.SetType(player.type);
 //}
 
-//void CRobotPredictor::updateTheirVision(int cycle, const VehicleInfoT & player, const MobileVisionT & ball, bool invert, int realNum) {
+//void CRobotPredictor::updateTheirVision(int cycle, const VehicleInfoT & player, const ObjectPoseT & ball, bool invert, int realNum) {
 //    const double x = invert ? -player.pos.x : player.pos.x;
 //    const double y = invert ? -player.pos.y : player.pos.y;
 //    double dir = 0;
@@ -435,7 +435,7 @@ bool CRobotPredictor::checkValid(int cycle, const CGeoPoint & pos) {
     }
     return true;
 }
-void CRobotPredictor::predictLost(int cycle, const MobileVisionT & ball) {
+void CRobotPredictor::predictLost(int cycle, const ObjectPoseT & ball) {
     //cout<<cycle<<" ";
     // 图像当前帧丢车后的处理办法
     if( _visionLogger.visionValid(cycle - 1)) {

Core/src/Vision/RobotPredictor.h

@@ -25,9 +25,9 @@ class CRobotPredictor {
     CRobotPredictor();
     CRobotPredictor (bool isHasRotaion);
     void updateCommand(int cycle, const CPlayerCommand* cmd); // 更新命令
-    void updateVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert); // 更新视觉信息
-    void updateOurVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum); // 更新视觉信息
-    void updateTheirVision(int cycle, const VehicleInfoT& player, const MobileVisionT& ball, bool invert, int realNum); // 更新视觉信息
+    void updateVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert); // 更新视觉信息
+    void updateOurVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum); // 更新视觉信息
+    void updateTheirVision(int cycle, const VehicleInfoT& player, const ObjectPoseT& ball, bool invert, int realNum); // 更新视觉信息
 
     CRobotCommandLogger& getCommandLogger() {
         return _commandLogger;
@@ -50,7 +50,7 @@ class CRobotPredictor {
     bool collideWithBall() const {
         return _collisionSimulator.hasCollision();    // 只是单纯地考虑是否相撞与碰撞区域
     }
-    const MobileVisionT& ballCollidedResult() const {
+    const ObjectPoseT& ballCollidedResult() const {
         return _collisionSimulator.ball();
     }
     void setIsHasRotation(bool isHasRot) {
@@ -59,7 +59,7 @@ class CRobotPredictor {
   protected:
 //    void updateRawVision(int cycle, double x, double y, double dir, bool seen);
     bool checkValid(int cycle, const CGeoPoint& pos);
-    void predictLost(int cycle, const MobileVisionT& ball); // 预测看不到的信息
+    void predictLost(int cycle, const ObjectPoseT& ball); // 预测看不到的信息
   private:
 //    FilteredObject _robotFilter; // KalmanFilter
 //    FilteredObject _robotRotationFilter; // 进行robot朝向滤波

Core/src/Vision/VisionModule.cpp

@@ -232,7 +232,7 @@ void CVisionModule::setNewVision() {
     /// @brief Step 2: 进行我方和对方机器人位置预测，关注滤波器
     /////////////////////////////////////////////////////////////////////////////
     //【#TODO】机器人碰撞检测器清空，
-    const MobileVisionT& thisBall = _ballPredictor.getData(_timeCycle);
+    const ObjectPoseT& thisBall = _ballPredictor.getData(_timeCycle);
     // 确定球员的信息是否需要反向，确保正确更新比赛双方球员的信息
     for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++ i) {
         const VehicleInfoT& ourPlayer = _info.player[0][i];

Core/src/Vision/VisionModule.h

@@ -82,7 +82,7 @@ class CVisionModule{
 		}
 	}
 
-    const MobileVisionT& ball() const {
+    const ObjectPoseT& ball() const {
         return _ballPredictor.getResult(_timeCycle);
     }
 
@@ -94,7 +94,7 @@ class CVisionModule{
         return _theirPlayerPredictor[num].getResult(cycle);
     }
 
-    const MobileVisionT& ball(int cycle) const {
+    const ObjectPoseT& ball(int cycle) const {
         return _ballPredictor.getResult(cycle);
     }
 

Core/src/WorldModel/WorldDefine.h

@@ -6,7 +6,7 @@
 /************************************************************************/
 class ObjectPoseT {
   public:
-    ObjectPoseT() : _valid(false), _pos(CGeoPoint(-9999, -9999)) { }
+    ObjectPoseT() : _valid(false), _pos(CGeoPoint(-9999, -9999)), _rawPos(CGeoPoint(-9999, -9999)) { }
     const CGeoPoint& Pos() const {
         return _pos;
     }
@@ -67,19 +67,7 @@ class ObjectPoseT {
     bool Valid() const {
         return _valid;
     }
-  private:
-    CGeoPoint _pos;
-    CVector _vel;
-    CVector _rawVel;
-    CVector _acc;
-    bool _valid;
-};
-/************************************************************************/
-/*                      VisionObjectT                                   */
-/************************************************************************/
-class VisionObjectT {
-  public:
-    VisionObjectT() : _rawPos(CGeoPoint(-9999, -9999)) { }
+
     const CGeoPoint& RawPos() const {
         return _rawPos;
     }
@@ -105,16 +93,19 @@ class VisionObjectT {
         _rawDir = rawdir;
     }
   private:
+    CGeoPoint _pos;
+    CVector _vel;
+    CVector _rawVel;
+    CVector _acc;
+    bool _valid;
+
     CGeoPoint _rawPos; // 视觉的原始信息，没有经过预测
     CGeoPoint _chipPredict; //挑球预测
     double _rawDir;
 };
-/************************************************************************/
-/*                       MobileVisionT                                  */
-/************************************************************************/
-class MobileVisionT : public ObjectPoseT, public VisionObjectT {
 
-};
+// using ObjectPoseT = ObjectPoseT;
+
 /************************************************************************/
 /*                        机器人姿态数据结构                               */
 /************************************************************************/
@@ -145,11 +136,11 @@ struct PlayerPoseT : public ObjectPoseT { // 目标信息
     double _rawRotVel;
 };
 /************************************************************************/
-/*                      PlayerTypeT                                     */
+/*                       PlayerVisionT                                  */
 /************************************************************************/
-class PlayerTypeT {
+class PlayerVisionT : public PlayerPoseT {
   public:
-    PlayerTypeT(): _type(0) {}
+    PlayerVisionT() : _type(0) {}
     void SetType(int t) {
         _type = t;
     }
@@ -159,12 +150,6 @@ class PlayerTypeT {
   private:
     int _type;
 };
-/************************************************************************/
-/*                       PlayerVisionT                                  */
-/************************************************************************/
-class PlayerVisionT : public PlayerPoseT, public VisionObjectT, public PlayerTypeT {
-
-};
 
 /************************************************************************/
 /*                        机器人能力数据结构                               */

Core/src/WorldModel/WorldModel.h

@@ -49,7 +49,7 @@ class CWorldModel{
 	const COptionModule* _pOption;
 	PlayerVisionT me;
 	PlayerVisionT he;
-	MobileVisionT ball;
+	ObjectPoseT ball;
 
 
 /************************************************** WorldModel_con ***************************************************/

Core/tactics/play/TestGetBall.lua

@@ -16,6 +16,10 @@ return {
         switch = function()
             local dist = (ball.pos() - player.pos("Assister")):mod()
             debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
+            debugEngine:gui_debug_x(player.pos("Assister"), param.GREEN)
+            debugEngine:gui_debug_msg(player.pos("Assister"), "Pos", param.GREEN)
+            debugEngine:gui_debug_x(player.rawPos("Assister"), param.PURPLE)
+            debugEngine:gui_debug_msg(player.rawPos("Assister"), "RawPos", param.PURPLE)
         end,
         Assister = task.touchKick(CGeoPoint(6000,0),nil,6000,true),
         -- Assister = task.stop(),