1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
#ifndef TEAMBALLLOCATORCANOPYCLUSTER_H
#define TEAMBALLLOCATORCANOPYCLUSTER_H

#include "ModuleFramework/Module.h"

#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugDrawings.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugParameterList.h"

#include "Representations/Infrastructure/FrameInfo.h"
#include "Representations/Infrastructure/FieldInfo.h"
#include "Representations/Modeling/TeamBallModel.h"
#include "Representations/Modeling/TeamMessage.h"
#include "Representations/Modeling/RobotPose.h"
#include "Representations/Modeling/TeamMessageNTP.h"
#include "Representations/Modeling/TeamMessagePlayersState.h"


BEGIN_DECLARE_MODULE(TeamBallLocatorCanopyCluster)
  PROVIDE(DebugModify)
  PROVIDE(DebugRequest)
  PROVIDE(DebugDrawings)
  PROVIDE(DebugParameterList)

  REQUIRE(FrameInfo)
  REQUIRE(FieldInfo)
  REQUIRE(TeamMessage)
  REQUIRE(RobotPose)
  REQUIRE(TeamMessageNTP)
  REQUIRE(TeamMessagePlayersState)

  PROVIDE(TeamBallModel)
END_DECLARE_MODULE(TeamBallLocatorCanopyCluster);


class TeamBallLocatorCanopyCluster : protected TeamBallLocatorCanopyClusterBase
{
public:
    class Parameters: public ParameterList
    {
    public:
        Parameters() : ParameterList("TeamBallLocatorCanopyCluster")
        {
            PARAMETER_REGISTER(maxBallAge) = 850; // in ms, wait at least two messages (approx.)
            PARAMETER_REGISTER(t1) = 1000; // in mm
            PARAMETER_REGISTER(t2) = 500; // in mm
            PARAMETER_REGISTER(maxTimeTbIsValidWithoutUpdate) = 2000; // in ms
            PARAMETER_REGISTER(ballsAreOnlyValidOnField) = true; // the received balls are only 'valid' if their inside the field boundries
            PARAMETER_REGISTER(enablePlayingCheck) = true; // whether the check, if a teammate is alive, active & playing should be used
            PARAMETER_REGISTER(enableNtpAdjustment) = true; // whether the ball age should be adjusted for the network latency
            syncWithConfig();
        }
        int maxBallAge;
        int t1; // loose distance
        int t2; // tight distance
        int maxTimeTbIsValidWithoutUpdate;
        bool ballsAreOnlyValidOnField;

        bool enablePlayingCheck;
        bool enableNtpAdjustment;
    } params;

    TeamBallLocatorCanopyCluster();
    ~TeamBallLocatorCanopyCluster();

    virtual void execute();

private:
    std::map<unsigned int, unsigned int> lastMessages;

    struct Ball
    {
        Vector2d pos;
        Vector2d sum;
        unsigned int size = 0;
        bool valid = true;

        Ball(const Vector2d& p) {
            pos = p;
            sum = p;
            ++size;
        }

        void add(const Ball& p) {
            sum += p.pos;
            ++size;
        }

        Vector2d center() {
            return sum / size;
        }
    };
};

#endif // TEAMBALLLOCATORCANOPYCLUSTER_H