00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <unistd.h>
00026
00027 #include <ssm.h>
00028 #include <ssmtype/spur-odometry.h>
00029 #include <odometry-ssm.h>
00030
00031 static SSM_sid odm_bs_sid;
00032 static SSM_sid odm_gl_sid;
00033
00034
00035 int OdometrySSM_init(void)
00036 {
00037
00038 if (!initSSM())
00039 return 0;
00040
00041 odm_bs_sid = openSSM("spur_odometry", 0, 0);
00042 odm_gl_sid = openSSM("spur_global", 0, 0);
00043 return 1;
00044 }
00045
00046
00047 int SSM_get_pos_GL_time(double time, double *x, double *y, double *theta)
00048 {
00049 Spur_Odometry odm1, odm2;
00050 int tid, tid2;
00051 double time1, time2;
00052 double rate;
00053
00054 tid = readSSM_time(odm_gl_sid, (char *)&odm1, time, &time1);
00055 tid2 = readSSM(odm_gl_sid, (char *)&odm2, &time2, tid + 1);
00056 if (tid2 < 0)
00057 {
00058 time2 = time1;
00059 odm2 = odm1;
00060 tid = readSSM(odm_gl_sid, (char *)&odm1, &time1, tid - 1);
00061 }
00062 if (fabs(time2 - time1) < 0.000001)
00063 return 0;
00064 if (fabs(time2 - time1) > 0.03)
00065 return 0;
00066
00067
00068
00069 rate = (time - time1) / (time2 - time1);
00070 *x = odm1.x + (odm2.x - odm1.x) * rate;
00071 *y = odm1.y + (odm2.y - odm1.y) * rate;
00072 while (odm2.theta - odm1.theta < -M_PI)
00073 odm2.theta += 2.0 * M_PI;
00074 while (odm2.theta - odm1.theta > M_PI)
00075 odm2.theta -= 2.0 * M_PI;
00076 *theta = odm1.theta + (odm2.theta - odm1.theta) * rate;
00077 while (odm2.theta - odm1.theta < -M_PI)
00078 odm2.theta += 2.0 * M_PI;
00079 while (odm2.theta - odm1.theta > M_PI)
00080 odm2.theta -= 2.0 * M_PI;
00081
00082 return 1;
00083 }
00084
00085
00086 int SSM_get_pos_BS_time(double time, double *x, double *y, double *theta)
00087 {
00088 Spur_Odometry odm1, odm2;
00089 int tid, tid2;
00090 double time1, time2;
00091 double rate;
00092
00093 tid = readSSM_time(odm_bs_sid, (char *)&odm1, time, &time1);
00094 tid2 = readSSM(odm_bs_sid, (char *)&odm2, &time2, tid + 1);
00095 if (tid2 < 0)
00096 {
00097 time2 = time1;
00098 odm2 = odm1;
00099 tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, tid - 1);
00100 }
00101 if (fabs(time2 - time1) < 0.000001)
00102 return 0;
00103 if (fabs(time2 - time1) > 0.03)
00104 return 0;
00105
00106
00107
00108 rate = (time - time1) / (time2 - time1);
00109 *x = odm1.x + (odm2.x - odm1.x) * rate;
00110 *y = odm1.y + (odm2.y - odm1.y) * rate;
00111 while (odm2.theta - odm1.theta < -M_PI)
00112 odm2.theta += 2.0 * M_PI;
00113 while (odm2.theta - odm1.theta > M_PI)
00114 odm2.theta -= 2.0 * M_PI;
00115 *theta = odm1.theta + (odm2.theta - odm1.theta) * rate;
00116 while (odm2.theta - odm1.theta < -M_PI)
00117 odm2.theta += 2.0 * M_PI;
00118 while (odm2.theta - odm1.theta > M_PI)
00119 odm2.theta -= 2.0 * M_PI;
00120
00121 return 1;
00122 }
00123
00124
00125 double SSM_get_pos_GL(double *x, double *y, double *theta)
00126 {
00127 Spur_Odometry odm1;
00128 int tid;
00129 double time1;
00130
00131 tid = readSSM(odm_gl_sid, (char *)&odm1, &time1, -1);
00132 if (tid < 0)
00133 return -1.0;
00134 *x = odm1.x;
00135 *y = odm1.y;
00136 *theta = odm1.theta;
00137
00138 return time1;
00139 }
00140
00141
00142 double SSM_get_pos_BS(double *x, double *y, double *theta)
00143 {
00144 Spur_Odometry odm1;
00145 int tid;
00146 double time1;
00147
00148 tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, -1);
00149 if (tid < 0)
00150 return -1.0;
00151
00152 *x = odm1.x;
00153 *y = odm1.y;
00154 *theta = odm1.theta;
00155
00156 return time1;
00157 }
00158
00159
00160 double SSM_get_vel(double *v, double *w)
00161 {
00162 Spur_Odometry odm1;
00163 int tid;
00164 double time1;
00165
00166 tid = readSSM(odm_bs_sid, (char *)&odm1, &time1, -1);
00167 if (tid < 0)
00168 return -1.0;
00169
00170 return time1;
00171 }
00172
00173 int SSM_near_pos_GL(double x, double y, double r)
00174 {
00175 double cx, cy, ctheta;
00176 double dist;
00177
00178 SSM_get_pos_GL(&cx, &cy, &ctheta);
00179 dist = sqrt((cx - x) * (cx - x) + (cy - y) * (cy - y));
00180 if (dist < r)
00181 return 1;
00182 else
00183 return 0;
00184 }
00185
00186 int SSM_near_ang(double th, double d)
00187 {
00188 double cx, cy, ctheta;
00189 double dist;
00190
00191 SSM_get_pos_GL(&cx, &cy, &ctheta);
00192 dist = ctheta - th;
00193 while (dist < -M_PI)
00194 dist += 2 * M_PI;
00195 while (dist > M_PI)
00196 dist -= 2 * M_PI;
00197 if (fabs(dist) < d)
00198 return 1;
00199 else
00200 return 0;
00201 }
00202
00203 int SSM_over_line(double x, double y, double theta)
00204 {
00205 return 0;
00206 }