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(); }
int main(int argc, char** argv) { /*if (argc < 1) { cout << "Need 1 argument: input_file" << endl; return -1; }*/ //ifstream infile(argv[1]); //declare particle filter and Location Visualizer for processing: ParticleFilter pf; pf.set_params(); //TODO: CHANGE! pf.add_particles(200,-300,0,0,1); //pf.add_particles(200); InputGenerator ig; LocationVisualizer lv; lv.draw_particle(pf.particles); lv.create_window(); lv.clear_buffer(); ifstream infile("straight_processed_patrick(1).txt");//"straight_processed_patrick_experimental.txt");//"straightwalk_binary_IPM_RnB.txt");//"straight_processed_patrick.txt"); string line; istringstream iss; vector<string> tokens; int counter = 0; if (infile.is_open()) { while (infile.good()) { getline(infile, line); tokens = split(line, ' '); cout<<"line size:"<<tokens.size()<<endl; for(int i = 0; i< tokens.size();i++){ cout<<tokens[i]<<endl; } if ((tokens.size() - 1) % 4 == 0 && tokens.size() > 1) { ////FILE PROCESSING ///new frame !!! string filename; vector<VisualFeature> vf; processed_line(&filename,&vf,tokens); filename += ".png"; filename = "./dataset_straightwalk/" + filename; cout<<"processing:"<<filename<<"coutner:"<<counter<<endl; counter++; //read file Mat image; image = imread(filename,1); namedWindow( "Display window", CV_WINDOW_AUTOSIZE );// Create a window for display. imshow( "Display window", image ); // Show our image inside it. /* vector<VisualFeature> vis_feat; ig.generate_features(counter * 20,0, 0,pf.feature_map, &vis_feat); ig.calculate_range_bearing(pf.feature_map,counter*20, 0, 0, vis_feat); for(int i = 0; i< vis_feat.size();i++){ cout<<"visual feature: "<<vis_feat[i].range<<" "<<vis_feat[i].bearing<<endl; } counter++;*/ ////////DO PARTICLE PROCESSING //shity implementation of odometry data: OdometryInformation of; of.rot = 0; of.x = 20; of.y = 0; pf.dynamic(of,vf); pf.resample(); lv.draw_particle(pf.particles); lv.create_window(); //make clean window (footballfield); lv.clear_buffer(); } } infile.close(); } return 0; }