ccparrow

afsim_observer插件

发表于

插件适用于AFSim2.9，将数据通过UDP协议以JSON格式发送到指定IP和端口，方便与其他系统集成和数据分析。

打开visual studio 2017，创建新项目wsf_observer_udp

文件结构

1
2
3
4
5
6
7
WsfObserverUdpExport.h​ -                导出宏定义
UdpSender.hpp                           UDP发送器头文件
UdpSender.cpp                           UDP发送器实现
WsfObserverUdp.hpp                      主观察者类头文件
WsfObserverUdp.cpp                      主观察者类实现
RegisterWsfObserverUdp.hpp              插件注册类
WsfObserverUdpRegistration.cpp          插件注册入口点

1-1、导出宏定义文件WsfObserverUdpExport

WsfObserverUdpExport.h

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
#ifndef WsfObserverUdpExportH
#define WsfObserverUdpExportH

#ifdef WSF_OBSERVER_UDP_STATIC_DEFINE
#  define WsfObserverUdpExport
#  define WsfObserverUdpNoExport
#else
#  ifndef WsfObserverUdpExport
#    ifdef wsfObserverUdp_EXPORTS
        /* We are building this library */
#      define WsfObserverUdpExport __declspec(dllexport)
#    else
        /* We are using this library */
#      define WsfObserverUdpExport __declspec(dllimport)
#    endif
#  endif

#  ifndef WsfObserverUdpNoExport
#    define WsfObserverUdpNoExport
#  endif
#endif

#ifndef WsfObserverUdpDeprecated
#  define WsfObserverUdpDeprecated __declspec(deprecated)
#endif

#ifndef WsfObserverUdpDeprecatedExport
#  define WsfObserverUdpDeprecatedExport WsfObserverUdpExport WsfObserverUdpDeprecated
#endif

#ifndef WsfObserverUdpDeprecatedNoExport
#  define WsfObserverUdpDeprecatedNoExport WsfObserverUdpNoExport WsfObserverUdpDeprecated
#endif

#if 0 /* DEFINE_NO_DEPRECATED */
#  ifndef WsfObserverUdpNoDeprecated
#    define WsfObserverUdpNoDeprecated
#  endif
#endif

#endif /* WsfObserverUdpExportH */

1-2、UDP发送器类UdpSender

UdpSender.hpp

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
#ifndef UdpSenderHPP
#define UdpSenderHPP

#include <string>
#ifdef _WIN32
    #include <winsock2.h>
    #include <ws2tcpip.h>
    #pragma comment(lib, "Ws2_32.lib")
#else
    #include <sys/socket.h>
    #include <arpa/inet.h>
    #include <unistd.h>
    #define SOCKET int
    #define INVALID_SOCKET (-1)
    #define SOCKET_ERROR (-1)
    #define closesocket close
#endif

class UdpSender {
public:
    UdpSender();
    ~UdpSender();

    bool Initialize(const std::string& targetIp, int targetPort);
    bool Send(const std::string& message);
    void Close();

private:
    SOCKET mSocket;
    struct sockaddr_in mServerAddr;
    bool mInitialized;
};

#endif // UdpSenderHPP

UdpSender.cpp

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
// 解决Windows SDK兼容性问题
#define DISABLEEXTENDEDALIGNEDSTORAGE
#define NOMINMAX
#define WIN32LEANANDMEAN

#include "UdpSender.hpp"
#include <cstring>
#include <iostream>

UdpSender::UdpSender() : mSocket(INVALID_SOCKET), mInitialized(false) {
#ifdef _WIN32
    WSADATA wsaData;
    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
        std::cerr << "WSAStartup failed." << std::endl;
    }
#endif
    std::memset(&mServerAddr, 0, sizeof(mServerAddr));
}

UdpSender::~UdpSender() {
    Close();
#ifdef _WIN32
    WSACleanup();
#endif
}

bool UdpSender::Initialize(const std::string& targetIp, int targetPort) {
    mSocket = socket(AF_INET, SOCK_DGRAM, 0);
    if (mSocket == INVALID_SOCKET) {
        std::cerr << "Failed to create UDP socket." << std::endl;
        return false;
    }

    mServerAddr.sin_family = AF_INET;
    mServerAddr.sin_port = htons(targetPort);

    if (inet_pton(AF_INET, targetIp.c_str(), &mServerAddr.sin_addr) <= 0) {
        std::cerr << "Invalid target IP address: " << targetIp << std::endl;
        closesocket(mSocket);
        mSocket = INVALID_SOCKET;
        return false;
    }

    mInitialized = true;
    std::cout << "UDP Sender initialized to " << targetIp << ":" << targetPort << std::endl;
    return true;
}

bool UdpSender::Send(const std::string& message) {
    if (!mInitialized || mSocket == INVALID_SOCKET) {
        std::cerr << "UDP Sender not initialized." << std::endl;
        return false;
    }

    int sentBytes = sendto(mSocket,
                          message.c_str(),
                          static_cast<int>(message.length()),
                          0,
                          (struct sockaddr*)&mServerAddr,
                          sizeof(mServerAddr));

    if (sentBytes == SOCKET_ERROR) {
        std::cerr << "Failed to send data via UDP." << std::endl;
        return false;
    }
    return true;
}

void UdpSender::Close() {
    if (mSocket != INVALID_SOCKET) {
        closesocket(mSocket);
        mSocket = INVALID_SOCKET;
    }
    mInitialized = false;
}

1-3、主观察者类WsfObserverUdp

WsfObserverUdp.hpp

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
#ifndef WsfObserverUdpHPP
#define WsfObserverUdpHPP

// 解决Windows SDK兼容性问题
#define DISABLEEXTENDEDALIGNEDSTORAGE
#define NOMINMAX
#define WIN32LEANANDMEAN

#include "WsfSimulationExtension.hpp"
#include "UtCallbackHolder.hpp"
#include "UdpSender.hpp"
#include <string>

struct UtPluginObjectParameters;
class WsfPlatform;
class WsfSensor;
class WsfTrack;

class WsfObserverUdp : public WsfSimulationExtension
{
public:
    WsfObserverUdp();
    ~WsfObserverUdp() noexcept override;

    bool Initialize() override;

private:
    void PlatformAdded(double aSimTime, WsfPlatform* aPlatformPtr);
    void PlatformDeleted(double aSimTime, WsfPlatform* aPlatformPtr);
    void SensorTrackUpdated(double aSimTime, WsfSensor* aSensorPtr, const WsfTrack* aTrackPtr);
    void AdvanceTime(double aSimTime);

private:
    UtCallbackHolder mCallbacks;
    double mPreSimTime = 0.0;
    UdpSender mUdpSender;
    
    // JSON格式化方法
    std::string FormatJsonMessage(const std::string& eventType, const std::string& data);
    std::string EscapeJsonString(const std::string& input);
};

#endif

WsfObserverUdp.cpp

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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
// 解决Windows SDK兼容性问题
#define DISABLEEXTENDEDALIGNEDSTORAGE
#define NOMINMAX
#define WIN32LEANANDMEAN

// 首先包含AFSim SDK头文件
#include "WsfApplication.hpp"
#include "observer/WsfPlatformObserver.hpp"
#include "observer/WsfTrackObserver.hpp"
#include "observer/WsfSimulationObserver.hpp"
#include "WsfPlatform.hpp"
#include "sensor/WsfSensor.hpp"
#include "WsfSimulation.hpp"
#include "WsfTrack.hpp"

// 然后包含标准库
#include <iostream>
#include <sstream>
#include <iomanip>

// 最后包含项目头文件
#include "WsfObserverUdp.hpp"

// UDP配置 - 在这里修改端口号和IP地址
const std::string UdpTargetIp = "127.0.0.1";
const int UdpTargetPort = 9999;

WsfObserverUdp::WsfObserverUdp()
{
}

WsfObserverUdp::~WsfObserverUdp() noexcept
{
}

bool WsfObserverUdp::Initialize()
{
    // 初始化UDP发送器
    if (!mUdpSender.Initialize(UdpTargetIp, UdpTargetPort)) {
        std::cerr << "Failed to initialize UDP Sender. Will use console output only." << std::endl;
    }

    // 注册回调函数
    mCallbacks.Add(WsfObserver::SensorTrackUpdated(&GetSimulation()).Connect(&WsfObserverUdp::SensorTrackUpdated, this));
    mCallbacks.Add(WsfObserver::SensorTrackInitiated(&GetSimulation()).Connect(&WsfObserverUdp::SensorTrackUpdated, this));
    mCallbacks.Add(WsfObserver::PlatformAdded(&GetSimulation()).Connect(&WsfObserverUdp::PlatformAdded, this));
    mCallbacks.Add(WsfObserver::PlatformDeleted(&GetSimulation()).Connect(&WsfObserverUdp::PlatformDeleted, this));
    mCallbacks.Add(WsfObserver::AdvanceTime(&GetSimulation()).Connect(&WsfObserverUdp::AdvanceTime, this));
    
    std::string initMsg = FormatJsonMessage("PluginLoaded", 
        "\"message\": \"WsfObserverUdp plugin loaded successfully\"");
    
    std::cout << initMsg << std::endl;
    mUdpSender.Send(initMsg);
    
    return true;
}

std::string WsfObserverUdp::EscapeJsonString(const std::string& input) {
    std::ostringstream ss;
    for (char c : input) {
        switch (c) {
            case '"': ss << "\\\""; break;
            case '\\': ss << "\\\\"; break;
            case '\b': ss << "\\b"; break;
            case '\f': ss << "\\f"; break;
            case '\n': ss << "\\n"; break;
            case '\r': ss << "\\r"; break;
            case '\t': ss << "\\t"; break;
            default:
                if (static_cast<unsigned char>(c) < 0x20 || static_cast<unsigned char>(c) == 0x7f) {
                    ss << "\\u" << std::hex << std::setw(4) << std::setfill('0') 
                       << static_cast<int>(static_cast<unsigned char>(c));
                } else {
                    ss << c;
                }
        }
    }
    return ss.str();
}

std::string WsfObserverUdp::FormatJsonMessage(const std::string& eventType, const std::string& data) {
    std::ostringstream json;
    json << "{";
    json << "\"eventType\": \"" << EscapeJsonString(eventType) << "\",";
    
    // 获取当前仿真时间
    double simTime = 0.0;
    try {
        simTime = GetSimulation().GetSimTime();
    } catch (...) {
        simTime = 0.0;
    }
    
    json << "\"timestamp\": " << std::fixed << std::setprecision(6) << simTime << ",";
    json << data;
    json << "}";
    return json.str();
}

void WsfObserverUdp::PlatformAdded(double aSimTime, WsfPlatform* aPlatformPtr)
{
    std::string platformData = "\"platform\": {";
    platformData += "\"name\": \"" + EscapeJsonString(aPlatformPtr->GetName()) + "\",";
    platformData += "\"type\": \"" + EscapeJsonString(aPlatformPtr->GetType()) + "\",";
    platformData += "\"index\": " + std::to_string(aPlatformPtr->GetIndex());
    platformData += "}";
    
    std::string message = FormatJsonMessage("PlatformAdded", platformData);
    
    std::cout << message << std::endl;
    mUdpSender.Send(message);
}

void WsfObserverUdp::PlatformDeleted(double aSimTime, WsfPlatform* aPlatformPtr)
{
    std::string platformData = "\"platform\": {";
    platformData += "\"name\": \"" + EscapeJsonString(aPlatformPtr->GetName()) + "\",";
    platformData += "\"type\": \"" + EscapeJsonString(aPlatformPtr->GetType()) + "\",";
    platformData += "\"index\": " + std::to_string(aPlatformPtr->GetIndex());
    platformData += "}";
    
    std::string message = FormatJsonMessage("PlatformDeleted", platformData);
    
    std::cout << message << std::endl;
    mUdpSender.Send(message);
}

void WsfObserverUdp::SensorTrackUpdated(double aSimTime, WsfSensor* aSensorPtr, const WsfTrack* aTrackPtr)
{
    double longitude, latitude, altitude;
    aTrackPtr->GetLocationLLA(latitude, longitude, altitude);
    
    std::string sensorData = "\"sensor\": {";
    sensorData += "\"name\": \"" + EscapeJsonString(aSensorPtr->GetName()) + "\",";
    sensorData += "\"platformIndex\": " + std::to_string(aSensorPtr->GetPlatform()->GetIndex());
    sensorData += "},";
    
    std::string trackData = "\"track\": {";
    trackData += "\"targetIndex\": " + std::to_string(aTrackPtr->GetTargetIndex()) + ",";
    trackData += "\"location\": {";
    trackData += "\"latitude\": " + std::to_string(latitude) + ",";
    trackData += "\"longitude\": " + std::to_string(longitude) + ",";
    trackData += "\"altitude\": " + std::to_string(altitude);
    trackData += "}}";
    
    std::string message = FormatJsonMessage("SensorTrackUpdated", sensorData + trackData);
    
    std::cout << message << std::endl;
    mUdpSender.Send(message);
}

void WsfObserverUdp::AdvanceTime(double aSimTime)
{
    double deltaTime = aSimTime - mPreSimTime;
    if (deltaTime < 0.0000001) return;
    mPreSimTime = aSimTime;

    std::string advanceMsg = FormatJsonMessage("AdvanceTime",
        "\"deltaTime\": " + std::to_string(deltaTime));
    
    std::cout << advanceMsg << std::endl;
    mUdpSender.Send(advanceMsg);

    // 获取场景所有平台信息
    int platformCount = GetSimulation().GetPlatformCount();
    for (int i = 0; i < platformCount; ++i)
    {
        auto platform = GetSimulation().GetPlatformEntry(i);
        
        // 获取位置信息（经纬高）
        auto lla = platform->GetLocationLLA();
        
        // 获取姿态信息（横滚、俯仰、航向）
        auto ned = platform->GetOrientationNED();

        std::string platformData = "\"platform\": {";
        platformData += "\"name\": \"" + EscapeJsonString(platform->GetName()) + "\",";
        platformData += "\"type\": \"" + EscapeJsonString(platform->GetType()) + "\",";
        platformData += "\"index\": " + std::to_string(platform->GetIndex()) + ",";
        
        platformData += "\"location\": {";
        platformData += "\"longitude\": " + std::to_string(lla.mLon) + ",";
        platformData += "\"latitude\": " + std::to_string(lla.mLat) + ",";
        platformData += "\"altitude\": " + std::to_string(lla.mAlt);
        platformData += "},";
        
        platformData += "\"orientation\": {";
        platformData += "\"roll\": " + std::to_string(ned.mPhi) + ",";
        platformData += "\"pitch\": " + std::to_string(ned.mTheta) + ",";
        platformData += "\"heading\": " + std::to_string(ned.mPsi);
        platformData += "}}";
        
        std::string message = FormatJsonMessage("PlatformInfo", platformData);
        
        std::cout << message << std::endl;
        mUdpSender.Send(message);
    }
}

1-4、插件注册类RegisterWsfObserverUdp

RegisterWsfObserverUdp.hpp

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
#ifndef RegisterWsfObserverUdpHPP
#define RegisterWsfObserverUdpHPP

// 解决Windows SDK兼容性问题
#define DISABLEEXTENDEDALIGNEDSTORAGE
#define NOMINMAX
#define WIN32LEANANDMEAN

#include "WsfObserverUdp.hpp"
#include "WsfScenarioExtension.hpp"
#include "WsfSimulation.hpp"
#include "UtMemory.hpp"

class RegisterWsfObserverUdp : public WsfScenarioExtension
{
public:
    ~RegisterWsfObserverUdp() noexcept override = default;

    void SimulationCreated(WsfSimulation& aSimulation) override
    {
        // Simulation对象创建完成后，注册Simulation扩展
        aSimulation.RegisterExtension("wsf_observer_udp", ut::make_unique<WsfObserverUdp>());
    }
};

#endif

1-5、插件注册入口点WsfObserverUdpRegistration

WsfObserverUdpRegistration.cpp

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
// 解决Windows SDK兼容性问题
#define DISABLEEXTENDEDALIGNEDSTORAGE
#define NOMINMAX
#define WIN32LEANANDMEAN

#include "WsfObserverUdpExport.h"
#include "RegisterWsfObserverUdp.hpp"
#include "WsfPlugin.hpp"
#include "WsfApplication.hpp"
#include "WsfApplicationExtension.hpp"
#include "UtMemory.hpp"

extern "C" {

WsfObserverUdpExport void WsfPluginVersion(UtPluginVersion& aVersion)
{
    aVersion = UtPluginVersion(
        WSF_PLUGIN_API_MAJOR_VERSION,
        WSF_PLUGIN_API_MINOR_VERSION,
        WSF_PLUGIN_API_COMPILER_STRING
    );
}

WsfObserverUdpExport void WsfPluginSetup(WsfApplication& aApplication)
{
    // 注册本插件工程
    // 因为RegisterWsfObserverUdp是Scenario扩展类，因此此处使用默认Application扩展
    aApplication.RegisterExtension("registerWsfObserverUdp", 
        ut::make_unique<WsfDefaultApplicationExtension<RegisterWsfObserverUdp>>());
}

}

2-1、配置类型修改为动态库dll

2-2、引用相关头文件

1
2
3
4
5
6
7
8
9
10
11
12
13
$(ProjectDir)afsimsdk\include
$(ProjectDir)afsimsdk\include\core\wsf\source
$(ProjectDir)afsimsdk\include\core\wsf\source\mover
$(ProjectDir)afsimsdk\include\core\wsf\source\sensor
$(ProjectDir)afsimsdk\include\core\wsf\source\observer
$(ProjectDir)afsimsdk\include\core\wsf_util\source
$(ProjectDir)afsimsdk\include\core\wsf\source\comm
$(ProjectDir)afsimsdk\include\core\wsf\source\processor
$(ProjectDir)afsimsdk\include\tools\dis\source
$(ProjectDir)afsimsdk\include\tools\genio\source
$(ProjectDir)afsimsdk\include\tools\util_script\source
$(ProjectDir)afsimsdk\include\tools\util\source
$(ProjectDir)afsimsdk\include\tools\packetio\source

2-3、引用库路径配置
也可以不配置，在引用库文件配置中，换成路径+库名。

1
$(ProjectDir)afsimsdk\lib

编译时会有link不过的错误，根据错误提示，追加相关库。

1
2
3
wsf.lib
util.lib
packetio.lib

2-4、预处理器宏定义
wsfplugin_EXPORTS 是上面【wsfplugin_export.h】输出动态库dll的宏。
PROMOTE_HARDWARE_EXCEPTIONS 是版本信息win_1929_64bit_release-hwe，不定义的话，编译器标记信息会缺少字段，与宿主工程对比编译器标记不通过，导致无法加载插件

1
2
3
wsfObserverUdp_EXPORTS
wsfplugin_EXPORTS
PROMOTE_HARDWARE_EXCEPTIONS

2-5、右键生成，生成路径如下

3-1、复制dll文件到AFSim插件目录

将wsf_observer_udp.dll文件拷贝到…\Release\wsf_plugins\路径下

3-2、插件加载测试

使用…\Release\mission.exe来验证插件是否能够正常加载(cmd命令行工具)

1
mission.exe -rt

使用mission.exe以实时仿真方式启动
AFSim自带的demos中的simple_scenario场景来测试。此场景中只有一个平台，名称是SimpleStriker，类型是BLUE_STRiKER

1
mission.exe -rt ../demos/simple_scenario/simple_scenario.txt
这里面的SensorTrackUpdated回调没有触发，是因为我们选择的场景不涉及Track，因此不会产生Track事件，也就无法触发此回调函数。

3-3、udp发送测试

python测试脚本

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
import socket
import json
import sys
from datetime import datetime

def udpJsonListener(port=9999):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(('0.0.0.0', port))
    
    print(f"开始监听 UDP JSON 数据，端口 {port}...")
    print("按 Ctrl+C 停止监听")
    print("-" * 80)
    
    try:
        while True:
            data, addr = sock.recvfrom(4096)
            try:
                message = data.decode('utf-8')
                jsonData = json.loads(message)
                
                # 格式化输出
                timestamp = datetime.now().strftime("%H:%M:%S.%f")[:-3]
                eventType = jsonData.get('eventType', 'Unknown')
                simTime = jsonData.get('timestamp', 0)
                
                print(f"[{timestamp}] {eventType} (SimTime: {simTime:.3f})")
                print(json.dumps(jsonData, indent=2, ensure_ascii=False))
                print("-" * 80)
                
            except json.JSONDecodeError as e:
                print(f"[{datetime.now().strftime('%H:%M:%S')}] JSON解析错误: {e}")
                print(f"原始数据: {data.decode('utf-8', errors='replace')}")
                print("-" * 80)
                
    except KeyboardInterrupt:
        print("\n停止监听")
    finally:
        sock.close()

if __name__ == "__main__":
    port = 9999
    if len(sys.argv) > 1:
        port = int(sys.argv[1])
    udpJsonListener(port)