int main() { // Setup for random number generator srand(time(NULL)); // Initialize our particle filter ParticleFilter filter; // Load occupancy map of wean hall and data from log filter.readMap(); filter.loadMapImage(); filter.readLog(); // Draw initial particles filter.drawParticles(); // Start the filter! for (int i = 1; i < filter.timestamps.size(); i++) { filter.motionModel(i); filter.updateWeights(i); filter.resampleParticles(); filter.visualize(); } std::cout << "Done!\n"; }
int main() { uWS::Hub h; // Set up parameters here double delta_t = 0.1; // Time elapsed between measurements [sec] double sensor_range = 50; // Sensor range [m] double sigma_pos[3] = { 0.3, 0.3, 0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]] double sigma_landmark[2] = { 0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]] // Read map data Map map; if (!read_map_data("../data/map_data.txt", map)) { cout << "Error: Could not open map file" << endl; return -1; } // Create particle filter ParticleFilter pf; h.onMessage([&pf, &map, &delta_t, &sensor_range, &sigma_pos, &sigma_landmark](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length, uWS::OpCode opCode) { // "42" at the start of the message means there's a websocket message event. // The 4 signifies a websocket message // The 2 signifies a websocket event if (length && length > 2 && data[0] == '4' && data[1] == '2') { auto s = hasData(std::string(data)); if (s != "") { auto j = json::parse(s); std::string event = j[0].get<std::string>(); if (event == "telemetry") { // j[1] is the data JSON object if (!pf.initialized()) { // Sense noisy position data from the simulator double sense_x = std::stod(j[1]["sense_x"].get<std::string>()); double sense_y = std::stod(j[1]["sense_y"].get<std::string>()); double sense_theta = std::stod(j[1]["sense_theta"].get<std::string>()); pf.init(sense_x, sense_y, sense_theta, sigma_pos); } else { // Predict the vehicle's next state from previous (noiseless // control) data. double previous_velocity = std::stod(j[1]["previous_velocity"].get<std::string>()); double previous_yawrate = std::stod(j[1]["previous_yawrate"].get<std::string>()); pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate); } // receive noisy observation data from the simulator // sense_observations in JSON format // [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}] vector<LandmarkObs> noisy_observations; string sense_observations_x = j[1]["sense_observations_x"]; string sense_observations_y = j[1]["sense_observations_y"]; std::vector<float> x_sense; std::istringstream iss_x(sense_observations_x); std::copy(std::istream_iterator<float>(iss_x), std::istream_iterator<float>(), std::back_inserter(x_sense)); std::vector<float> y_sense; std::istringstream iss_y(sense_observations_y); std::copy(std::istream_iterator<float>(iss_y), std::istream_iterator<float>(), std::back_inserter(y_sense)); for (int i = 0; i < x_sense.size(); i++) { LandmarkObs obs; obs.x = x_sense[i]; obs.y = y_sense[i]; noisy_observations.push_back(obs); } // Update the weights and resample pf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map); pf.resample(); // Calculate and output the average weighted error of the particle // filter over all time steps so far. vector<Particle> particles = pf.particles; int num_particles = particles.size(); double highest_weight = -1.0; Particle best_particle; double weight_sum = 0.0; for (int i = 0; i < num_particles; ++i) { if (particles[i].weight > highest_weight) { highest_weight = particles[i].weight; best_particle = particles[i]; } weight_sum += particles[i].weight; } cout << "highest w " << highest_weight << endl; cout << "average w " << weight_sum / num_particles << endl; json msgJson; msgJson["best_particle_x"] = best_particle.x; msgJson["best_particle_y"] = best_particle.y; msgJson["best_particle_theta"] = best_particle.theta; // Optional message data used for debugging particle's sensing and // associations msgJson["best_particle_associations"] = pf.getAssociations(best_particle); msgJson["best_particle_sense_x"] = pf.getSenseX(best_particle); msgJson["best_particle_sense_y"] = pf.getSenseY(best_particle); auto msg = "42[\"best_particle\"," + msgJson.dump() + "]"; // std::cout << msg << std::endl; ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); } } else { std::string msg = "42[\"manual\",{}]"; ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); } } }); // We don't need this since we're not using HTTP but if it's removed the // program // doesn't compile :-( h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data, size_t, size_t) { const std::string s = "<h1>Hello world!</h1>"; if (req.getUrl().valueLength == 1) { res->end(s.data(), s.length()); } else { // i guess this should be done more gracefully? res->end(nullptr, 0); } }); h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) { std::cout << "Connected!!!" << std::endl; }); h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code, char *message, size_t length) { ws.close(); std::cout << "Disconnected" << std::endl; }); int port = 4567; if (h.listen(port)) { std::cout << "Listening to port " << port << std::endl; } else { std::cerr << "Failed to listen to port" << std::endl; return -1; } h.run(); }