rb_sm.c
Go to the documentation of this file.
1 #include "rb_sm.h"
2 #include <stdio.h>
3 #include <gsl/gsl_nan.h>
4 #include <options/options.h>
5 
8 
9 void rb_sm_init_journal(const char*journal_file) {
10  jj_set_stream(open_file_for_writing(journal_file));
11 /* sm_journal_open(journal_file);*/
12 }
13 
15  FILE * s = jj_get_stream();
16  if(s) fclose(s);
17 
18  jj_set_stream(0);
19 }
20 
21 void rb_sm_odometry(double x, double y, double theta){
24  rb_sm_params.first_guess[2]=theta;
25 }
26 
27 void rb_sm_odometry_cov(double cov_x, double cov_y, double cov_theta){
28 
29 }
30 
31 struct option* ops = 0;
32 
33 int rb_sm_set_configuration(const char*name, const char*value) {
34  if(!ops) {
35  ops = options_allocate(30);
36  sm_options(&rb_sm_params, ops);
37  }
38 
39  if(!options_try_pair(ops, name, value)) {
40 
41  return 0;
42  } else
43  return 1;
44 }
45 
46 const char *rb_result_to_json() {
47  static char buf[5000];
49  strcpy(buf, jo_to_string(jo));
50  jo_free(jo);
51  return buf;
52 }
53 
54 int rb_sm_icp() {
56  return rb_sm_result.valid;
57 }
58 
59 int rb_sm_gpm() {
61  return rb_sm_result.valid;
62 }
63 
64 void rb_set_laser_ref(const char*s) {
66 /* fprintf(stderr, "Set laser_ref to %p\n ", rb_sm_params.laser_ref );*/
67 }
68 
69 void rb_set_laser_sens(const char*s) {
71 /* fprintf(stderr, "Set laser_sens to %p\n ", rb_sm_params.laser_sens );*/
72 }
73 
74 void rb_sm_cleanup() {
79 }
80 
81 LDP string_to_ld(const char*s) {
82  JO jo = json_parse(s);
83  if(!jo) {
84  fprintf(stderr, "String passed from Ruby is invalid JSON: \n\n%s\n", s);
85  return 0;
86  }
87  LDP ld = json_to_ld(jo);
88  if(!ld) {
89  fprintf(stderr, "String passed from Ruby is valid JSON, "
90  "but can't load laser_data. \n\n%s\n", s);
91  return 0;
92  }
93  jo_free(jo);
94  return ld;
95 }
96 
void rb_set_laser_ref(const char *s)
Definition: rb_sm.c:64
void rb_set_laser_sens(const char *s)
Definition: rb_sm.c:69
LDP json_to_ld(JO jo)
void sm_icp(struct sm_params *input, struct sm_result *output)
Definition: icp.c:29
int rb_sm_set_configuration(const char *name, const char *value)
Definition: rb_sm.c:33
#define jo_to_string
struct sm_result rb_sm_result
Definition: rb_sm.c:7
int rb_sm_gpm()
Definition: rb_sm.c:59
LDP string_to_ld(const char *s)
Definition: rb_sm.c:81
struct option * ops
Definition: rb_sm.c:31
FILE * open_file_for_writing(const char *filename)
Definition: utils.c:25
void rb_sm_cleanup()
Definition: rb_sm.c:74
int valid
Definition: algos.h:140
void rb_sm_close_journal()
Definition: rb_sm.c:14
Definition: options.h:49
void ld_free(LDP ld)
Definition: laser_data.c:87
int rb_sm_icp()
Definition: rb_sm.c:54
JO result_to_json(struct sm_params *p, struct sm_result *r)
struct option * options_allocate(int n)
const char * name
Definition: options.h:51
void rb_sm_odometry(double x, double y, double theta)
Definition: rb_sm.c:21
LDP laser_ref
Definition: algos.h:14
LDP laser_sens
Definition: algos.h:16
const char * rb_result_to_json()
Definition: rb_sm.c:46
double first_guess[3]
Definition: algos.h:19
JO json_parse(const char *str)
struct sm_params rb_sm_params
Definition: rb_sm.c:6
void rb_sm_init_journal(const char *journal_file)
Definition: rb_sm.c:9
void rb_sm_odometry_cov(double cov_x, double cov_y, double cov_theta)
Definition: rb_sm.c:27
double x[3]
Definition: algos.h:143
int options_try_pair(struct option *ops, const char *name, const char *value)
Definition: options.c:228
void sm_gpm(struct sm_params *input, struct sm_result *output)
Definition: gpm.c:11
FILE * jj_get_stream()
Definition: json_journal.c:113
void jj_set_stream(FILE *f)
Definition: json_journal.c:109
char buf[100]
Definition: ld_recover.c:87
void sm_options(struct sm_params *p, struct option *ops)
Definition: sm_options.c:6
#define jo_free


csm
Author(s): Andrea Censi
autogenerated on Tue May 11 2021 02:18:23