rb_sm.c
Go to the documentation of this file.
00001 #include "rb_sm.h"
00002 #include <stdio.h>
00003 #include <gsl/gsl_nan.h>
00004 #include <options/options.h>
00005 
00006 struct sm_params rb_sm_params; 
00007 struct sm_result rb_sm_result;
00008 
00009 void rb_sm_init_journal(const char*journal_file) {
00010         jj_set_stream(open_file_for_writing(journal_file));
00011 /*      sm_journal_open(journal_file);*/
00012 }
00013 
00014 void rb_sm_close_journal() {
00015         FILE * s = jj_get_stream();
00016         if(s) fclose(s);
00017         
00018         jj_set_stream(0);
00019 }
00020 
00021 void rb_sm_odometry(double x, double y, double theta){
00022         rb_sm_params.first_guess[0]=x;
00023         rb_sm_params.first_guess[1]=y;
00024         rb_sm_params.first_guess[2]=theta;
00025 }
00026 
00027 void rb_sm_odometry_cov(double cov_x, double cov_y, double cov_theta){
00028         
00029 }
00030 
00031 struct option* ops = 0;
00032 
00033 int rb_sm_set_configuration(const char*name, const char*value) {
00034         if(!ops) { 
00035                 ops = options_allocate(30);
00036                 sm_options(&rb_sm_params, ops);
00037         }
00038         
00039         if(!options_try_pair(ops, name, value)) {
00040 
00041                 return 0;
00042         } else
00043         return 1;
00044 }
00045 
00046 const char *rb_result_to_json() {
00047         static char buf[5000];
00048         JO jo = result_to_json(&rb_sm_params, &rb_sm_result);
00049         strcpy(buf, jo_to_string(jo));
00050         jo_free(jo);
00051         return buf;
00052 }
00053 
00054 int rb_sm_icp() {
00055         sm_icp(&rb_sm_params, &rb_sm_result);
00056         return rb_sm_result.valid;
00057 }
00058 
00059 int rb_sm_gpm() {       
00060         sm_gpm(&rb_sm_params, &rb_sm_result);
00061         return rb_sm_result.valid;
00062 }
00063 
00064 void rb_set_laser_ref(const char*s) {
00065         rb_sm_params.laser_ref = string_to_ld(s);
00066 /*      fprintf(stderr, "Set laser_ref to %p\n ", rb_sm_params.laser_ref );*/
00067 }
00068 
00069 void rb_set_laser_sens(const char*s) {
00070         rb_sm_params.laser_sens = string_to_ld(s);
00071 /*      fprintf(stderr, "Set laser_sens to %p\n ", rb_sm_params.laser_sens );*/
00072 }
00073 
00074 void rb_sm_cleanup() {
00075         if(rb_sm_params.laser_ref)
00076         ld_free(rb_sm_params.laser_ref);
00077         if(rb_sm_params.laser_sens)
00078         ld_free(rb_sm_params.laser_sens);
00079 }
00080 
00081 LDP string_to_ld(const char*s) {
00082         JO jo = json_parse(s);
00083         if(!jo) {
00084                 fprintf(stderr, "String passed from Ruby is invalid JSON: \n\n%s\n", s);
00085                 return 0;
00086         }
00087         LDP ld = json_to_ld(jo);
00088         if(!ld) {
00089                 fprintf(stderr, "String passed from Ruby is valid JSON, "
00090                         "but can't load laser_data. \n\n%s\n", s);
00091                 return 0;
00092         }
00093         jo_free(jo);
00094         return ld;
00095 }
00096 


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29