libodomssm.c
Go to the documentation of this file.
1 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19 // SOFTWARE.
20 
21 #include <math.h>
22 #include <stdio.h>
23 #include <stdlib.h>
24 #include <string.h>
25 #include <unistd.h>
26 
27 #include <ssm.h>
28 #include <ssmtype/spur-odometry.h>
29 #include <odometry-ssm.h>
30 
31 static SSM_sid odm_bs_sid;
32 static SSM_sid odm_gl_sid;
33 
34 /* poetとのメッセージ通信を開始する */
36 {
37  /* オープン */
38  if (!initSSM())
39  return 0;
40 
41  odm_bs_sid = openSSM("spur_odometry", 0, 0);
42  odm_gl_sid = openSSM("spur_global", 0, 0);
43  return 1;
44 }
45 
46 /* 線形補間で位置取得 */
47 int SSM_get_pos_GL_time(double time, double *x, double *y, double *theta)
48 {
49  Spur_Odometry odm1, odm2;
50  int tid, tid2;
51  double time1, time2;
52  double rate;
53 
54  tid = readSSM_time(odm_gl_sid, (char *)&odm1, time, &time1);
55  tid2 = readSSM(odm_gl_sid, (char *)&odm2, &time2, tid + 1);
56  if (tid2 < 0)
57  { /* 次のがなければ前のでなんとか。 */
58  time2 = time1;
59  odm2 = odm1;
60  tid = readSSM(odm_gl_sid, (char *)&odm1, &time1, tid - 1);
61  }
62  if (fabs(time2 - time1) < 0.000001)
63  return 0;
64  if (fabs(time2 - time1) > 0.03)
65  return 0;
66 
67  // printf("%f %f %f\n",time-time1,time2-time, time2-time1);
68 
69  rate = (time - time1) / (time2 - time1);
70  *x = odm1.x + (odm2.x - odm1.x) * rate;
71  *y = odm1.y + (odm2.y - odm1.y) * rate;
72  while (odm2.theta - odm1.theta < -M_PI)
73  odm2.theta += 2.0 * M_PI;
74  while (odm2.theta - odm1.theta > M_PI)
75  odm2.theta -= 2.0 * M_PI;
76  *theta = odm1.theta + (odm2.theta - odm1.theta) * rate;
77  while (odm2.theta - odm1.theta < -M_PI)
78  odm2.theta += 2.0 * M_PI;
79  while (odm2.theta - odm1.theta > M_PI)
80  odm2.theta -= 2.0 * M_PI;
81 
82  return 1;
83 }
84 
85 /* 線形補間で位置取得 */
86 int SSM_get_pos_BS_time(double time, double *x, double *y, double *theta)
87 {
88  Spur_Odometry odm1, odm2;
89  int tid, tid2;
90  double time1, time2;
91  double rate;
92 
93  tid = readSSM_time(odm_bs_sid, (char *)&odm1, time, &time1);
94  tid2 = readSSM(odm_bs_sid, (char *)&odm2, &time2, tid + 1);
95  if (tid2 < 0)
96  { /* 次のがなければ前のでなんとか。 */
97  time2 = time1;
98  odm2 = odm1;
99  tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, tid - 1);
100  }
101  if (fabs(time2 - time1) < 0.000001)
102  return 0;
103  if (fabs(time2 - time1) > 0.03)
104  return 0;
105 
106  // printf("%f %f %f\n",time-time1,time2-time, time2-time1);
107 
108  rate = (time - time1) / (time2 - time1);
109  *x = odm1.x + (odm2.x - odm1.x) * rate;
110  *y = odm1.y + (odm2.y - odm1.y) * rate;
111  while (odm2.theta - odm1.theta < -M_PI)
112  odm2.theta += 2.0 * M_PI;
113  while (odm2.theta - odm1.theta > M_PI)
114  odm2.theta -= 2.0 * M_PI;
115  *theta = odm1.theta + (odm2.theta - odm1.theta) * rate;
116  while (odm2.theta - odm1.theta < -M_PI)
117  odm2.theta += 2.0 * M_PI;
118  while (odm2.theta - odm1.theta > M_PI)
119  odm2.theta -= 2.0 * M_PI;
120 
121  return 1;
122 }
123 
124 /* */
125 double SSM_get_pos_GL(double *x, double *y, double *theta)
126 {
127  Spur_Odometry odm1;
128  int tid;
129  double time1;
130 
131  tid = readSSM(odm_gl_sid, (char *)&odm1, &time1, -1);
132  if (tid < 0)
133  return -1.0;
134  *x = odm1.x;
135  *y = odm1.y;
136  *theta = odm1.theta;
137 
138  return time1;
139 }
140 
141 /* */
142 double SSM_get_pos_BS(double *x, double *y, double *theta)
143 {
144  Spur_Odometry odm1;
145  int tid;
146  double time1;
147 
148  tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, -1);
149  if (tid < 0)
150  return -1.0;
151 
152  *x = odm1.x;
153  *y = odm1.y;
154  *theta = odm1.theta;
155 
156  return time1;
157 }
158 
159 /* */
160 double SSM_get_vel(double *v, double *w)
161 {
162  Spur_Odometry odm1;
163  int tid;
164  double time1;
165 
166  tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, -1);
167  if (tid < 0)
168  return -1.0;
169 
170  return time1;
171 }
172 
173 int SSM_near_pos_GL(double x, double y, double r)
174 {
175  double cx, cy, ctheta;
176  double dist;
177 
178  SSM_get_pos_GL(&cx, &cy, &ctheta);
179  dist = sqrt((cx - x) * (cx - x) + (cy - y) * (cy - y));
180  if (dist < r)
181  return 1;
182  else
183  return 0;
184 }
185 
186 int SSM_near_ang(double th, double d)
187 {
188  double cx, cy, ctheta;
189  double dist;
190 
191  SSM_get_pos_GL(&cx, &cy, &ctheta);
192  dist = ctheta - th;
193  while (dist < -M_PI)
194  dist += 2 * M_PI;
195  while (dist > M_PI)
196  dist -= 2 * M_PI;
197  if (fabs(dist) < d)
198  return 1;
199  else
200  return 0;
201 }
202 
203 int SSM_over_line(double x, double y, double theta)
204 {
205  return 0;
206 }
int SSM_near_ang(double th, double d)
Definition: libodomssm.c:186
static SSM_sid odm_gl_sid
Definition: libodomssm.c:32
double SSM_get_pos_GL(double *x, double *y, double *theta)
Definition: libodomssm.c:125
double SSM_get_vel(double *v, double *w)
Definition: libodomssm.c:160
int SSM_get_pos_BS_time(double time, double *x, double *y, double *theta)
Definition: libodomssm.c:86
double SSM_get_pos_BS(double *x, double *y, double *theta)
Definition: libodomssm.c:142
static SSM_sid odm_bs_sid
Definition: libodomssm.c:31
int SSM_get_pos_GL_time(double time, double *x, double *y, double *theta)
Definition: libodomssm.c:47
int SSM_near_pos_GL(double x, double y, double r)
Definition: libodomssm.c:173
int SSM_over_line(double x, double y, double theta)
Definition: libodomssm.c:203
int OdometrySSM_init(void)
Definition: libodomssm.c:35


yp-spur
Author(s):
autogenerated on Fri May 7 2021 02:12:17