import java.util.ArrayList; import java.util.List; import java.util.Scanner;

import org.json.JSONArray; import org.json.JSONObject;

public class ParticleFilter {

public static String hasData(String s) {
    int found_null = s.indexOf("null");
    int b1 = s.indexOf("[");
    int b2 = s.indexOf("]");
    if (found_null != -1) {
        return "";
    } else if (b1 != -1 && b2 != -1) {
        return s.substring(b1, b2 + 1);
    }
    return "";
}

public static void main(String[] args) {
    double delta_t = 0.1;
    double sensor_range = 50;

    double[] sigma_pos = {0.3, 0.3, 0.01};
    double[] sigma_landmark = {0.3, 0.3};

    Map map = new Map();
    if (!map.read_map_data("../data/map_data.txt")) {
        System.out.println("Error: Could not open map file");
        System.exit(-1);
    }

    ParticleFilter pf = new ParticleFilter();

    // WebSocket server setup
    uWS.Hub h = new uWS.Hub();

    h.onMessage((ws, message, length, opCode) -> {
        if (length > 2 && message[0] == '4' && message[1] == '2') {
            String s = hasData(new String(message));

            if (!s.equals("")) {
                JSONObject j = new JSONObject(s);
                String event = j.getString("0");

                if (event.equals("telemetry")) {
                    JSONObject data = j.getJSONObject("1");

                    if (!pf.initialized()) {
                        double sense_x = Double.parseDouble(data.getString("sense_x"));
                        double sense_y = Double.parseDouble(data.getString("sense_y"));
                        double sense_theta = Double.parseDouble(data.getString("sense_theta"));

                        pf.init(sense_x, sense_y, sense_theta, sigma_pos);
                    } else {
                        double previous_velocity = Double.parseDouble(data.getString("previous_velocity"));
                        double previous_yawrate = Double.parseDouble(data.getString("previous_yawrate"));

                        pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate);
                    }

                    String sense_observations_x = data.getString("sense_observations_x");
                    String sense_observations_y = data.getString("sense_observations_y");

                    List<Double> x_sense = new ArrayList<>();
                    Scanner scanner_x = new Scanner(sense_observations_x).useDelimiter(",");
                    while (scanner_x.hasNextDouble()) {
                        x_sense.add(scanner_x.nextDouble());
                    }

                    List<Double> y_sense = new ArrayList<>();
                    Scanner scanner_y = new Scanner(sense_observations_y).useDelimiter(",");
                    while (scanner_y.hasNextDouble()) {
                        y_sense.add(scanner_y.nextDouble());
                    }

                    List<LandmarkObs> noisy_observations = new ArrayList<>();
                    for (int i = 0; i < x_sense.size(); i++) {
                        LandmarkObs obs = new LandmarkObs();
                        obs.x = x_sense.get(i);
                        obs.y = y_sense.get(i);
                        noisy_observations.add(obs);
                    }

                    pf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map);
                    pf.resample();

                    List<Particle> particles = pf.particles;
                    int num_particles = particles.size();
                    double highest_weight = -1.0;
                    Particle best_particle = null;
                    double weight_sum = 0.0;
                    for (int i = 0; i < num_particles; i++) {
                        Particle particle = particles.get(i);
                        if (particle.weight > highest_weight) {
                            highest_weight = particle.weight;
                            best_particle = particle;
                        }

                        weight_sum += particle.weight;
                    }

                    System.out.println("highest w " + highest_weight);
                    System.out.println("average w " + weight_sum / num_particles);

                    JSONObject msgJson = new JSONObject();
                    msgJson.put("best_particle_x", best_particle.x);
                    msgJson.put("best_particle_y", best_particle.y);
                    msgJson.put("best_particle_theta", best_particle.theta);

                    JSONArray associations = new JSONArray();
                    associations.put(pf.getAssociations(best_particle));
                    msgJson.put("best_particle_associations", associations);

                    JSONArray sense_x = new JSONArray();
                    sense_x.put(pf.getSenseCoord(best_particle, "X"));
                    msgJson.put("best_particle_sense_x", sense_x);

                    JSONArray sense_y = new JSONArray();
                    sense_y.put(pf.getSenseCoord(best_particle, "Y"));
                    msgJson.put("best_particle_sense_y", sense_y);

                    String msg = "42[\"best_particle\"," + msgJson.toString() + "]";
                    ws.send(msg.getBytes(), opCode);
                }
            } else {
                String msg = "42[\"manual\",{}]";
                ws.send(msg.getBytes(), opCode);
            }
        }
    });

    h.onConnection((ws, req) -> {
        System.out.println("Connected!!!");
    });

    h.onDisconnection((ws, code, message, length) -> {
        ws.close();
        System.out.println("Disconnected");
    });

    int port = 4567;
    if (h.listen(port)) {
        System.out.println("Listening to port " + port);
    } else {
        System.err.println("Failed to listen to port");
        System.exit(-1);
    }

    h.run();
}

// Rest of the code...

}

convert c++ code to java below#include mathh#include uWSuWSh#include iostream#include string#include jsonhpp#include particle_filterh for convenienceusing nlohmannjson;using stdstring;using stdvector;

原文地址: https://www.cveoy.top/t/topic/jaHQ 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录