// $Id: test_bootstrapfilter.cpp,v 2.43 2005/01/06 14:30:53 kgadeyne Exp $// Copyright (C) 2003 Klaas Gadeyne <klaas dot gadeyne at mech dot kuleuven dot ac dot be>//  // This program is free software; you can redistribute it and/or modify// it under the terms of the GNU General Public License as published by// the Free Software Foundation; either version 2 of the License, or// (at your option) any later version.//  // This program is distributed in the hope that it will be useful,// but WITHOUT ANY WARRANTY; without even the implied warranty of// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the// GNU General Public License for more details.//  // You should have received a copy of the GNU General Public License// along with this program; if not, write to the Free Software// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.//  // Socket code added by Jan Callewaert/* Primary demonstration program for the Bayesian Filtering Library.   Mobile robot localization with a particle filter.  Use   ./testbootstrapfilter.out --help    to see the possible command line arguments...*//* IMPORTANT NOTE FROM THE AUTHOR *//* If you look at this "tutorial/demo" program for the first time, you   might get overwhelmed by the amount of code.  However, there is a   lot of code contained in this example which has nothing to do with   the estimation process in se and only with visualisation,   debugging, ...   All that code is put between #ifdef statements.  For first time   study/understanding of this application, you can better eliminate   this code.   Emacs users:   - M-x hide-ifdef-mode to enter ifdef minor mode   - C-c @ C-d will hide code between ifdef statements (notice the   ellipsis if it worked)   - C-c @ C-s will show hidden code between ifdef statements*/#include <filter/bootstrapfilter.h>#include <filter/asirfilter.h>#include <filter/EKparticlefilter.h>#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>#include <pdf/analyticconditionalgaussian.h>#include <pdf/gaussian.h>#include <matrix_wrapper.h>#include <vector_wrapper.h>#include <iostream>#include <fstream>// The following headers are only neccessary when using opendx and// sending the data over a socket#define _BOOTSTRAPFILTER_// #define _ASIRFILTER_// #define _EK_PARTICLEFILTLER_#ifdef __STREAM__#ifdef __OPENDX__#include <cstdlib>#ifdef __SOCKET__#include <sys/types.h>#include <sys/socket.h>#include <sys/wait.h>#include <netinet/in.h>#include <arpa/inet.h>#include <unistd.h>#include <errno.h>#endif // __SOCKET__#include <string>#endif // __OPENDX__#endif // __STREAM__#include "parser.h"#include "mobile_robot_particlefilter.h"using namespace std;using namespace MatrixWrapper;#ifdef __SOCKET__// Socket descriptions#define DEFAULT_PORT 3490  // Default port#define BACKLOG 10         // how many pending connections queue will hold#define MSGSIZE 100        // Size of a buffer when sending a file#define SLPTIME 2#ifdef __OPENDX__// Set socket connectionvoid init_connection( int * sin_size,int * sockfd, int port ){  int yes=1;  struct sockaddr_in my_addr;    // my address information  if ((*sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {    cerr << "socket error\n";    exit(1);  }  if (setsockopt(*sockfd,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(int)) == -1) {    cerr << "setsockopt error\n";    exit(1);  }          my_addr.sin_family = AF_INET;         // host byte order  my_addr.sin_port = htons(port);       // short, network byte order  my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill with my IP  memset(&(my_addr.sin_zero), '\0', 8); // zero the rest of the struct  // Bind socket descriptor  if (bind(*sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr))      == -1) {    cerr << "bind error\n";    exit(1);  }  // Listen to socket  if (listen(*sockfd, BACKLOG) == -1) {    cerr << "listen error\n";    exit(1);  }  cout << "connection made to port " << port << endl;}#endif // __SOCKET__#endif // __OPENDX__int main(int argc, char **argv){  cerr << "==================================================\n"       << "Test of the BootstrapFilter"       << "Mobile robot localisation example\n"       << "=================================================="       << endl;  // For the parsing of command line variables  /* Set argument defaults */  struct arguments arguments;  arguments.verbose = 0;  arguments.num_samples = DEFAULT_NUM_SAMPLES;  arguments.num_time_steps = DEFAULT_NUM_TIME_STEPS;   arguments.resample_period = DEFAULT_RESAMPLE_PERIOD;  arguments.linear_speed = DEFAULT_LINEAR_SPEED;  arguments.rot_speed = DEFAULT_ROT_SPEED;  arguments.rico_wall = DEFAULT_RICO_WALL;  arguments.offset_wall = DEFAULT_OFFSET_WALL;  arguments.name = "new_experiment";#ifdef __SOCKET__  arguments.port = DEFAULT_PORT;#endif // __SOCKET__  /* Where the magic happens */  int ret = argp_parse (&argp, argc, argv, 0, 0, &arguments);  if (ret != 0)     {      cerr << "Parsing error" << endl;      return ret;    }  cout << "****************************\n"       << "*  name: " << arguments.name << endl       << "*  num_samples: " << arguments.num_samples << endl       << "*  num_time_steps: " << arguments.num_time_steps << endl       << "*  resample_period: " << arguments.resample_period << endl       << "*  linear_speed: " << arguments.linear_speed << endl       << "*  rot_speed: " << arguments.rot_speed << endl#ifdef __SOCKET__       << "*  port: " << arguments.port << endl#endif // __SOCKET__       << "*  rico_wall: " << arguments.rico_wall << endl       << "*  offset_wall: " << arguments.offset_wall << endl       << "****************************\n\n";  // Measurement Noise  wall_ct = 1/(sqrt(pow(arguments.rico_wall,2.0) + 1));  mu_meas_noise = arguments.offset_wall * wall_ct;#ifdef __STREAM__#ifdef __OPENDX__  char dxfile_location[128] = "data_visualisation/filter_samples.dx";#ifdef __SOCKET__  char tempdx_location[128] = "data_visualisation/temp.dx";  // Socket parameters  int sockfd, new_fd;            // listen on sock_fd, new connection on new_fd  struct sockaddr_in their_addr; // connector's address information  int sin_size;  // Setup the socket  init_connection( &sin_size, &sockfd, arguments.port );  // Accept the connection  sin_size = sizeof(struct sockaddr_in);  if ( (new_fd = accept(sockfd, (struct sockaddr *)&their_addr,			(socklen_t *)&sin_size)) == -1)     {      cerr << "accept";    }  if (arguments.verbose == 1)    {      cout << "server: got connection from " 	   << inet_ntoa(their_addr.sin_addr) << endl;    }  close(sockfd); // child doesn't need the listener  char buf[20];  char *start_name = "STR_NAM";  char *start_rico_wall = "STR_RIC";  char *start_offset_wall = "STR_OFF";  char *start_msgsize = "STR_MSG";  char *start_serie = "STR_SER";  char *end_serie = "END_SER";  char *start_update = "STR_UPD";//   char *start_file = "STR_FIL";//   char *end_file = "END_FIL";  char *end = "END";    // Send the name of the experiment  if (send(new_fd,start_name, strlen(start_name)+1, 0) == -1)    cerr << "send\n";    sleep(SLPTIME);  if (send(new_fd, arguments.name, strlen(arguments.name)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  // First send the rico_wall  if (send(new_fd,start_rico_wall, strlen(start_rico_wall)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  gcvt(arguments.rico_wall,10,buf); // you have to convert double to string  cerr << "strlen(buf): " << strlen(buf) << endl;  if (send(new_fd, buf, strlen(buf)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  // Then send the offset_wall  if (send(new_fd,start_offset_wall, strlen(start_offset_wall)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  gcvt(arguments.offset_wall,10,buf);  if (send(new_fd, buf, strlen(buf)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  // We send the file in buffers of size MSGSIZE  if (send(new_fd,start_msgsize, strlen(start_msgsize)+1, 0) == -1)    cerr << "send\n";  sleep(SLPTIME);  sprintf(buf,"%i",MSGSIZE);  cerr << "strlen(msgsize): " << strlen(buf) << endl;  if (send(new_fd, buf, strlen(buf)+1,0) == -1 )    cerr << "send\n";  sleep(SLPTIME);  char stream[MSGSIZE]; // contains stream read from file	    ifstream file;        // ifstream that contains file to open for writing#endif // __SOCKET__#endif // __OPENDX__#endif // __STREAM__  init_system_model();  init_measurement_model();  init_prior();#ifdef __STREAM__#ifdef __OPENDX__  ofstream dxnative_samplefile(dxfile_location, ios::out);  dxnative_samplefile.precision(10);  dxnative_samplefile.width(10);  if (dxnative_samplefile)    {      dxnative_samplefile << "# Data of the samples in DX native format\n" << endl;    }  else { cerr << "MAIN: Error writing to DX files. Stopping program" << endl; exit(-1); }#endif // __OPENDX__#endif // __STREAM__  // Creating a MONTE CARLO density with these samples#ifdef __STREAM__  cerr << "MAIN: Creating the Prior density" << endl;#endif // __STREAM__  init_mc_prior(arguments.num_samples);#ifdef __STREAM__  ofstream priorsamples_f("data_visualisation/prior_samples.dat", ios::out);  list<Sample<ColumnVector> >::iterator prior_it;  if (priorsamples_f)    {#ifdef __OCTAVE__      priorsamples_f << "# name: priorsamplesvector\n# type: matrix\n"		     << "# rows: " << arguments.num_samples*(STATE_SIZE) << "\n"		     << "# columns: 1\n" << endl;#endif // __OCTAVE__      for (prior_it = prior_samples.begin() ; prior_it != prior_samples.end() ; prior_it++)	{	  priorsamples_f << prior_it->ValueGet();	}    }  else     {       cerr << "MAIN: Error writing to file" << endl;    }  priorsamples_f.close();#endif // __STREAM__  /*****************************************   * SIMULATION OF INPUTS AND MEASUREMENTS *   *****************************************/  simulate_measurements(arguments.num_time_steps,			arguments.linear_speed,			arguments.rot_speed);#ifdef __STREAM__  cerr << "MAIN: Starting the simulation of the measurements" << endl;  ofstream simulation_f("data_visualisation/simulation.dat", ios::out);  if (simulation_f)    {#ifdef __OCTAVE__      simulation_f << "# name: simulation_states\n# type: matrix\n"		   << "# rows: " << (STATE_SIZE *(arguments.num_time_steps)) << "\n"		   << "# columns: 1\n" << endl;#endif // __OCTAVE_      for (current_time=0; 	   current_time < arguments.num_time_steps;	   current_time++)	{  // #define WEIGHT 1	  simulation_f << States[current_time]; // << WEIGHT << " \n";	}    }  else { cerr << "MAIN: Error writing to file" << endl; }#endif // __STREAM// #ifdef __DEBUG__  //  cerr << "\nMAIN: states 0\n" << States[0] << endl;//  double probability = meas_model.ProbabilityGet(Measurements[0],// 						States[0]);//  cerr << "\nMAIN: Meas 0 : "<< Measurements[0]//       << "Prob : " << probability << "\n";// #endif //  __DEBUG__// #ifdef __STREAM__//   // #define WEIGHT 1//        simulation_f << States[current_time+1]; // << WEIGHT << " \n";// #endif //  __STREAM__  /******************************   * Construction of the Filter *   ******************************/#ifdef __STREAM__  cerr << "\nMAIN: Construction of the Bootstrapfilter: Post MCPdf will be created" << endl;#endif // __STREAM__#ifdef _ASIRFILTER_  cerr << "MAIN: Using the Auxiliary Particle Filter" << endl;  filter_p = new ASIRFilter<ColumnVector,ColumnVector> (Prior, 							0,							DEFAULT_RESAMPLE_THRESHOLD,							DEFAULT_RS);#endif#ifdef _EK_PARTICLEFILTLER_  cerr << "MAIN: Using the Extended Kalman Particle Filter" << endl;  filter_p = new EKParticleFilter(Prior, 0, DEFAULT_RESAMPLE_THRESHOLD,				  DEFAULT_RS);#endif