libodomssm.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
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 /* poetとのメッセージ通信を開始する */
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   // printf("%f %f %f\n",time-time1,time2-time, time2-time1);
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   // printf("%f %f %f\n",time-time1,time2-time, time2-time1);
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 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19