Target_Planer.cpp

00001 #include "Target_Planer.h"
00002 
00003 Target_Planer::Target_Planer(double *Pathx, double *Pathy, uint16_t N, double R)
00004 {
00005     for(uint16_t i = 0; i < N; i++) {
00006         Path_.x.push_back(Pathx[i]);
00007         Path_.y.push_back(Pathy[i]);
00008     }
00009     Path_.N = N;
00010 
00011     RobotPos_.x = -999.999;
00012     RobotPos_.y = -999.999;
00013 
00014     TargetPos_.x = -999.999;
00015     TargetPos_.y = -999.999;
00016     
00017     TargetAng_ = -999.999;
00018 
00019     R_ = R;
00020     del_ = 0.001;    // in meter
00021     i_min_ = 65535;  // biggest number for uint16_t = 2^16-1 = 65535
00022 }
00023 
00024 Target_Planer::~Target_Planer() {}
00025 
00026 void Target_Planer::initialize(double RobotPosx, double RobotPosy)
00027 {
00028     RobotPos_.x = RobotPosx;
00029     RobotPos_.y = RobotPosy;
00030     i_min_ = findClosestPointOnPath(Path_, RobotPos_);
00031 }
00032 
00033 void Target_Planer::update(double RobotPosx, double RobotPosy)
00034 {
00035     // update robot position
00036     RobotPos_.x = RobotPosx;
00037     RobotPos_.y = RobotPosy;
00038 
00039     /* project on path */
00040 
00041     // check if next point is closer and update if yes
00042     double d_min = calcDistSquared(Path_, RobotPos_, i_min_);
00043     if(i_min_ < Path_.N-1) {
00044         double d_ip1 = calcDistSquared(Path_, RobotPos_, i_min_+1);
00045         if(d_ip1 <= d_min) {
00046             d_min = d_ip1;
00047             i_min_ = i_min_ + 1;
00048         }
00049     }
00050     // since root function is monotonic calculate square root after evaluation
00051     d_min = sqrt(d_min);
00052 
00053     // already treat case where no projection lies on a path segment:
00054     // if it is a corner and no closer valid projection then this already is the
00055     // projection point of the robot to the path (corner point)
00056     double sx = Path_.x[i_min_];
00057     double sy = Path_.y[i_min_];
00058     double dl1 = d_min;
00059     uint16_t i_next = i_min_;
00060     double a_i;
00061 
00062     if(i_min_ == 0) {
00063         projectOnPath(Path_, RobotPos_, 1, &a_i, &sx, &sy, &dl1);
00064         if(a_i < 0) {
00065             i_next = 0; // robot must be before first path segment
00066         } else {
00067             i_next = 1; // robot projection is on first path segment
00068         }
00069     } else if(i_min_ == Path_.N-1) {
00070         projectOnPath(Path_, RobotPos_, Path_.N-2, &a_i, &sx, &sy, &dl1);
00071         i_next = Path_.N-1;
00072     } else {
00073         // check if projection is valid and which projection point is the
00074         // closest -> actual projection on path
00075         for(uint16_t i = i_min_-1; i <= i_min_; i++) {
00076             double sx_i, sy_i, dl1_i;
00077             double dp_i_norm = calcNormPathsegment_i(Path_, i);
00078             projectOnPath(Path_, RobotPos_, i, &a_i, &sx_i, &sy_i, &dl1_i);
00079             // true if robot is on path segment
00080             if(a_i > 0 - del_ && a_i < dp_i_norm + del_) {
00081                 // true if distance to projection point is shorter
00082                 if(dl1_i < dl1) {
00083                     sx = sx_i;
00084                     sy = sy_i;
00085                     dl1 = dl1_i;
00086                     i_next = i + 1;
00087                 }
00088             }
00089         }
00090     }
00091     
00092     /* based on this projection find target point along path (intersection with radius R) */
00093     
00094     if(dl1 >= R_ - del_) {
00095         TargetPos_.x = sx;
00096         TargetPos_.y = sy;
00097     } else {
00098         double i_ahead = i_next;
00099         double px_i = sx;
00100         double py_i = sy;
00101         double px_ip1 = Path_.x[i_ahead];
00102         double py_ip1 = Path_.y[i_ahead];
00103         bool isvalid;
00104         double x_intsec_valid, y_intsec_valid;
00105         while(1) {
00106             findIntersectionWithPath(px_i, py_i, px_ip1, py_ip1, RobotPos_.x, RobotPos_.y, &isvalid, &x_intsec_valid, &y_intsec_valid);
00107             if(isvalid) {
00108                 TargetPos_.x = x_intsec_valid;
00109                 TargetPos_.y = y_intsec_valid;
00110                 break;
00111             } else {
00112                 i_ahead = i_ahead + 1;
00113                 if(i_ahead > Path_.N-1) { // TargetPos has to be the endpoint
00114                     i_ahead = Path_.N-1;
00115                     TargetPos_.x = Path_.x[i_ahead];
00116                     TargetPos_.y = Path_.y[i_ahead];
00117                     break;
00118                 }
00119                 px_i   = Path_.x[i_ahead-1];
00120                 py_i   = Path_.y[i_ahead-1];
00121                 px_ip1 = Path_.x[i_ahead];
00122                 py_ip1 = Path_.y[i_ahead];
00123             }
00124         }
00125     }
00126     
00127     // restore R for special cases before first point or after last point
00128     double dTargetPosx = TargetPos_.x - RobotPos_.x;
00129     double dTargetPosy = TargetPos_.y - RobotPos_.y;
00130     double dTargetPos_norm = sqrt(dTargetPosx*dTargetPosx + dTargetPosy*dTargetPosy);
00131     if(dTargetPos_norm > R_ + del_) {
00132         TargetPos_.x = RobotPos_.x + R_*dTargetPosx/dTargetPos_norm;
00133         TargetPos_.y = RobotPos_.y + R_*dTargetPosy/dTargetPos_norm;
00134     }
00135 
00136     // calculate target angle
00137     TargetAng_ = atan2(TargetPos_.y - RobotPos_.y, TargetPos_.x - RobotPos_.x);
00138     
00139 }
00140 
00141 void Target_Planer::readTargetPos(double *TargetPosx, double *TargetPosy)
00142 {
00143     *TargetPosx = TargetPos_.x;
00144     *TargetPosy = TargetPos_.y;
00145 }
00146 
00147 void Target_Planer::readTargetAng(double *TargetAng)
00148 {
00149     *TargetAng = TargetAng_;
00150 }
00151 
00152 void Target_Planer::readTarget(double *TargetPosx, double *TargetPosy, double *TargetAng)
00153 {
00154     *TargetPosx = TargetPos_.x;
00155     *TargetPosy = TargetPos_.y;
00156     *TargetAng = TargetAng_;
00157 }
00158 
00159 double Target_Planer::readTargetPosx()
00160 {
00161     return TargetPos_.x;
00162 }
00163     
00164 double Target_Planer::readTargetPosy()
00165 {
00166     return TargetPos_.y;
00167 }
00168 
00169 double Target_Planer::readTargetAng()
00170 {
00171     return TargetAng_;
00172 }
00173 
00174 void Target_Planer::updateAndReturnTarget(double RobotPosx, double RobotPosy, double *TargetPosx, double *TargetPosy, double *TargetAng)
00175 {
00176     update(RobotPosx, RobotPosy);
00177     *TargetPosx = TargetPos_.x;
00178     *TargetPosy = TargetPos_.y;
00179     *TargetAng  = TargetAng_;
00180 }
00181 
00182 uint16_t Target_Planer::findClosestPointOnPath(PATH Path, ROBOTPOS RobotPos)
00183 {
00184     double   d_min = 1000000;
00185     uint16_t i_min = 0;
00186     for(uint16_t i = 0; i < Path.N; i++) {
00187         double dx = Path.x[i] - RobotPos.x;
00188         double dy = Path.y[i] - RobotPos.y;
00189         // since root function is monotonic no need to evaluate root itself
00190         double d = dx*dx + dy*dy;
00191         if(i == 0) {
00192             d_min = d;
00193             i_min = 0;
00194         } else {
00195             if(d <= d_min) {
00196                 d_min = d;
00197                 i_min = i;
00198             }
00199         }
00200     }
00201     return i_min;
00202 }
00203 
00204 double Target_Planer::calcDistSquared(PATH Path, ROBOTPOS RobotPos, uint16_t i)
00205 {
00206     double dx = Path.x[i] - RobotPos.x;
00207     double dy = Path.y[i] - RobotPos.y;
00208     return dx*dx + dy*dy;
00209 }
00210 
00211 double Target_Planer::calcNormPathsegment_i(PATH Path, uint16_t i)
00212 {
00213     double dx = Path.x[i+1] - Path.x[i];
00214     double dy = Path.y[i+1] - Path.y[i];
00215     return sqrt(dx*dx + dy*dy);
00216 }
00217 
00218 void Target_Planer::projectOnPath(PATH Path, ROBOTPOS RobotPos, uint16_t i, double *a_i, double *sx_i, double *sy_i, double *dl1_i)
00219 {
00220     // delta pathsegment
00221     double dpx_i = Path.x[i+1] - Path.x[i];
00222     double dpy_i = Path.y[i+1] - Path.y[i];
00223 
00224     // norm delta pathsegment
00225     double s = 1/sqrt(dpx_i*dpx_i + dpy_i*dpy_i);
00226     double dpx_in = s*dpx_i;
00227     double dpy_in = s*dpy_i;
00228 
00229     // a_i:   scalar length of robot position projected on pathsegment
00230     // sx_i:  x-value of projection point s
00231     // sy_i:  y-value of projection point s
00232     // dl1_i: scalar length of robot position to projection point s (length of tangent)
00233     *a_i = (RobotPos.x - Path.x[i])*dpx_in + (RobotPos.y - Path.y[i])*dpy_in;
00234     *sx_i = Path.x[i] + *a_i * dpx_in;
00235     *sy_i = Path.y[i] + *a_i * dpy_in;
00236     double dx = *sx_i - RobotPos.x;
00237     double dy = *sy_i - RobotPos.y;
00238     *dl1_i = sqrt(dx*dx + dy*dy);
00239 }
00240 
00241 void Target_Planer::findIntersectionWithPath(double px_i, double py_i, double px_ip1, double py_ip1, double rx, double ry, bool *isvalid, double *x_intsec_valid, double *y_intsec_valid)
00242 {
00243     // transform data on pathsegment so that origin is in waypoint_0 and x points towards waypoint_1 
00244     double wp1x, wp1y, x_act, y_act, s_phi, c_phi;
00245     transformDataOnPath(px_i, py_i, px_ip1, py_ip1, rx, ry, &wp1x, &wp1y, &x_act, &y_act, &s_phi, &c_phi);
00246     
00247     // coefficients for intersection calculus
00248     double radic = R_*R_ - wp1y*wp1y + 2.0*wp1y*y_act - y_act*y_act;;
00249     
00250     // intersection candidates
00251     double sqrt_radic;
00252     uint8_t possible_solutions;
00253     if(radic > 0.0) {
00254         // 2 intersections
00255         sqrt_radic = sqrt(radic);
00256         possible_solutions = 2;
00257     } else {
00258         // 1 contact point or tangent point
00259         sqrt_radic = 0.0;
00260         possible_solutions = 1;
00261     }
00262     
00263     // check if the solution is valid with bounded box
00264     double dist_to_wp1_min = 1000000; // big number
00265     double sign_radicand = 1.0;
00266     double x_intsec_valid_temp = -999.999;
00267     double y_intsec_valid_temp = -999.999;
00268     bool   isvalid_temp = false;
00269     for(uint8_t i = 0; i < possible_solutions-1; i++) {
00270         double lowb_x, uppb_x, lowb_y, uppb_y;
00271         if(0.0 < wp1x) {
00272             lowb_x = 0.0;
00273             uppb_x = wp1x;
00274         } else {
00275             lowb_x = wp1x;
00276             uppb_x = 0.0;
00277         }
00278         if(0.0 < wp1y) {
00279             lowb_y = 0.0;
00280             uppb_y = wp1y;
00281         } else {
00282             lowb_y = wp1y;
00283             uppb_y = 0.0;
00284         }
00285         double x_intsec_cand = x_act + sign_radicand*sqrt_radic; // intersection candidate x
00286         double y_intsec_cand = wp1y;                             // intersection candidate y
00287         if(lowb_x - del_ < x_intsec_cand && x_intsec_cand < uppb_x + del_) {
00288             if(lowb_y - del_ < y_intsec_cand && y_intsec_cand < uppb_y + del_) {
00289                 if((x_intsec_cand - x_act)*(x_intsec_cand - x_act) + (y_intsec_cand - y_act)*(y_intsec_cand - y_act) <= (R_ + del_)*(R_ + del_)) {
00290                     // valid if it shorter than R, del treats numerical uncertainty
00291                     isvalid_temp = true;
00292                     double dist_to_wp1 = sqrt((wp1x - x_intsec_cand)*(wp1x - x_intsec_cand) + (wp1y - y_intsec_cand)*(wp1y - y_intsec_cand));
00293                     if(dist_to_wp1 <= dist_to_wp1_min) {
00294                         // if there are 2 solutions we have to guarantee that we take the closer one ahead (we creep along the path)
00295                         dist_to_wp1_min = dist_to_wp1;
00296                         x_intsec_valid_temp = x_intsec_cand;
00297                         y_intsec_valid_temp = y_intsec_cand;
00298                     }
00299                 }
00300             }
00301         }
00302         sign_radicand = -1.0;
00303     }
00304     
00305     // inverse transformation of solution if valid
00306     if(isvalid_temp) {
00307         inverseTransformDataOnPath(px_i, py_i, s_phi, c_phi, &x_intsec_valid_temp, &y_intsec_valid_temp);
00308     }
00309     
00310     // copy variables
00311     *isvalid = isvalid_temp;
00312     *x_intsec_valid = x_intsec_valid_temp;
00313     *y_intsec_valid = y_intsec_valid_temp;
00314     
00315 }
00316 
00317 void Target_Planer::transformDataOnPath(double px_i, double py_i, double px_ip1, double py_ip1, double rx, double ry, double *wp1x, double *wp1y, double *x_act, double *y_act, double *s_phi, double *c_phi)
00318 {
00319     double phi = atan2(py_ip1 - py_i, px_ip1 - px_i);
00320     double sin_phi = sin(phi);
00321     double cos_phi = cos(phi);
00322     
00323     *wp1x  =  cos_phi*(px_ip1 - px_i) + sin_phi*(py_ip1 - py_i);
00324     *wp1y  = -sin_phi*(px_ip1 - px_i) + cos_phi*(py_ip1 - py_i);
00325     *x_act =  cos_phi*(rx - px_i) + sin_phi*(ry - py_i);
00326     *y_act = -sin_phi*(rx - px_i) + cos_phi*(ry - py_i);  
00327     *s_phi = sin_phi;
00328     *c_phi = cos_phi;
00329 }
00330 
00331 void Target_Planer::inverseTransformDataOnPath(double px_i, double py_i, double s_phi, double c_phi, double *x_intsec_valid_temp, double *y_intsec_valid_temp)
00332 {
00333     double x_temp = *x_intsec_valid_temp;
00334     double y_temp = *y_intsec_valid_temp;
00335     *x_intsec_valid_temp =  c_phi*x_temp - s_phi*y_temp + px_i;
00336     *y_intsec_valid_temp =  s_phi*x_temp + c_phi*y_temp + py_i;
00337 }
00338 
00339 /* debug */
00340 
00341 double Target_Planer::returnPathxAtIndex(int i)
00342 {
00343     return Path_.x[i];
00344 }
00345 
00346 double Target_Planer::returnPathyAtIndex(int i)
00347 {
00348     return Path_.y[i];
00349 }
00350 
00351 uint16_t Target_Planer::returnPathLength()
00352 {
00353     return Path_.N;
00354 }
00355 
00356 uint16_t Target_Planer::returnClosestPointOnPath(double RobotPosx, double RobotPosy)
00357 {
00358     RobotPos_.x = RobotPosx;
00359     RobotPos_.y = RobotPosy;
00360     return findClosestPointOnPath(Path_, RobotPos_);
00361 }
00362 
00363 uint16_t Target_Planer::return_i_min_()
00364 {
00365     return i_min_;
00366 }
00367 
00368