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;
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...
}
原文地址: https://www.cveoy.top/t/topic/jaHQ 著作权归作者所有。请勿转载和采集!