udacity 粒子滤波(PF)定位-c++程序

udacity 粒子滤波(PF)定位程序，c++版本，根据网上的C++程序改写，方便大家参考。

（1）Udacity参考代码

https://github.com/matriculus/Udacity-Particle-Filter

（2）uadcity模拟器(term2)

https://github.com/udacity/self-driving-car-sim/releases

(3) 参考文章

uWebSockets的使用(二):uWebSockets的编译和使用-CSDN博客

uWebSockets的使用(一):uSockets的编译和使用(一)-CSDN博客

一、关于<uWS/uWS.h>

这个头文件是uWebSockets早期采用的版本，现在已经不用了。

二、关于原版粒子滤波（PF）程序

原版PF程序采用<uWS/uWS.h>,应该也比较早了。

三、关于新版粒子滤波（PF）程序

采用最新的uWebSockets程序。

四、关于json.hpp

因为vs环境下语言标准采用ISO C++20标准，要下载最新的json.hpp。

五、参考的uWebSockets样例程序

参考uWebSockets样例程序为examples下的EchoServer.cpp。

六、用到的库

uwebsocket.lib
zlibwapi.lib
libcrypto.lib
libssl.lib
libuv.lib
DbgHelp.lib
uslib.lib（uSockets库）
Iphlpapi.lib
kernel32.lib
psapi.lib
userenv.lib

七、改写的main函数
#include <math.h>
#include "App.h"
#include <iostream>
#include <string>
#include "json.hpp"
#include "particle_filter.h"

// for convenience
using nlohmann::json;
using std::string;
using std::vector;

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
    auto found_null = s.find("null");
    auto b1 = s.find_first_of("[");
    auto b2 = s.find_first_of("]");
    if (found_null != string::npos) {
        return "";
    }
    else if (b1 != string::npos && b2 != string::npos) {
        return s.substr(b1, b2 - b1 + 1);
    }
    return "";
}

/* This is a simple WebSocket echo server example.
 * You may compile it with "WITH_OPENSSL=1 make" or with "make" */

int main() {
    /* ws->getUserData returns one of these */

    struct PerSocketData {
        /* Fill with user data */
    };

    // Set up parameters here
    double delta_t = 0.1;  // Time elapsed between measurements [sec]
    double sensor_range = 50;  // Sensor range [m]

    // GPS measurement uncertainty [x [m], y [m], theta [rad]]
    double sigma_pos[3] = { 0.3, 0.3, 0.01 };
    // Landmark measurement uncertainty [x [m], y [m]]
    double sigma_landmark[2] = { 0.3, 0.3 };

    // Read map data
    Map map;
    if (!read_map_data("D:/Auto/particle_localization/data/map_data.txt", map)) {
        std::cout << "Error: Could not open map file" << std::endl;
        return -1;
    }

    // Create particle filter
    ParticleFilter pf;

    /* Keep in mind that uWS::SSLApp({options}) is the same as uWS::App() when compiled without SSL support.
     * You may swap to using uWS:App() if you don't need SSL */
    uWS::App({
        /* There are example certificates in uWebSockets.js repo */
        .key_file_name = "misc/key.pem",
        .cert_file_name = "misc/cert.pem",
        .passphrase = "1234"
        }).ws<PerSocketData>("/*", {
            /* Settings */
            .compression = uWS::CompressOptions(uWS::DEDICATED_COMPRESSOR_4KB | uWS::DEDICATED_DECOMPRESSOR),
            .maxPayloadLength = 100 * 1024 * 1024,
            .idleTimeout = 16,
            .maxBackpressure = 100 * 1024 * 1024,
            .closeOnBackpressureLimit = false,
            .resetIdleTimeoutOnSend = false,
            .sendPingsAutomatically = true,
            /* Handlers */
            .upgrade = nullptr,
            .open = [](auto*/*ws*/) {
                /* Open event here, you may access ws->getUserData() which points to a PerSocketData struct */

            },
            .message = [&pf, &delta_t,&map, &sensor_range, &sigma_pos, &sigma_landmark]
            (auto* ws, std::string_view message, uWS::OpCode opCode) {
                /* This is the opposite of what you probably want; compress if message is LARGER than 16 kb
                 * the reason we do the opposite here; compress if SMALLER than 16 kb is to allow for
                 * benchmarking of large message sending without compression */
                //ws->send(message, opCode, message.length() < 1