ClearpathDemoTools.cpp
Go to the documentation of this file.
00001 #include "ClearpathDemoTools.h"
00002 
00003 #include "Vector3.h"
00004 #include "bestfit.h"
00005 #include <cmath>
00006 #include <cstring>
00007 #include <cstdio>
00008 #include <cstdlib>
00009 #include <ctime>
00010 
00011 /******************************************************************
00012 
00013                             STATIC HELPERS
00014 
00015 *******************************************************************/
00016 
00017 // Uses 3 random points to construct a plane (normal vector + offset)
00018 bool ClearpathDemoTools::GetPlaneFromRnd3(PointCloud* cloud, double* normal_coeff)
00019 {
00020     int i1, i2, i3, num;
00021     i1 = i2 = i3 = 0;
00022     num = cloud->points.size();
00023     
00024     // Atleast 3 points must exist
00025     if (num > 3) 
00026     {
00027         Vector3 a1, a2, a3;
00028         
00029         // Get a random number
00030         i1 = i2 = i3 = rand() % num;
00031         a1.x = cloud->points[i1].x; a1.y = cloud->points[i1].y; a1.z = cloud->points[i1].z;
00032 
00033         // While points are the same or too close together, find a new second point
00034         while (i2 == i1 || (a1-a2).Length() < RANSAC_PLANE_SEPDIST) { 
00035             i2 = rand() % num; 
00036             a2.x = cloud->points[i2].x; a2.y = cloud->points[i2].y; a2.z = cloud->points[i2].z;
00037             //TODO timeout incase points are all close? 
00038         }
00039 
00040         // While points are the same or too close together, find a new third point
00041         while ( i3 == i1 || i3 == i2 || (a3-a1).Length() < RANSAC_PLANE_SEPDIST || (a3-a2).Length() < RANSAC_PLANE_SEPDIST) { 
00042             i3 = rand() % num;
00043             a3.x = cloud->points[i3].x; a3.y = cloud->points[i3].y; a3.z = cloud->points[i3].z;
00044             //TODO timeout incase points are all close? 
00045         }
00046 
00047         // Get Normal Vector
00048         Vector3 ax, ay, az;
00049         ax = a2 - a1; ax.Normalize();
00050         ay = (a3 - a1) - ax*(ax.Dot(a3-a1)); ay.Normalize();
00051         az = ax.Cross(ay);
00052         
00053         // Flip to make it positive
00054         if (az.y < 0.0)
00055             az = az*-1.0;
00056 
00057         // Find the offset of the plane
00058         double mag = az.Dot(a1);
00059 
00060         // Fill Result
00061         normal_coeff[0] = az.x;
00062         normal_coeff[1] = az.y;
00063         normal_coeff[2] = az.z;
00064         normal_coeff[3] = mag;
00065 
00066     } else {
00067         return false;
00068     }
00069     return true;
00070 }
00071 
00072 // Find the random sample consensus plane
00073 // normal_coeff + match_coeff offer the option to reject planes that are too different from an initial guess
00074 bool ClearpathDemoTools::PlaneRANSAC(PointCloud* cloud, double* normal_coeff, bool match_coeff)
00075 {
00076     // Make random
00077     srand ( time(NULL) );
00078 
00079     // Variables
00080     int iter_count = 0;
00081     int bestcount = 0;    
00082     double bestnorm[4];
00083 
00084     // For X Iterations
00085     while (iter_count < RANSAC_MAX_ITER)
00086     {   
00087         iter_count++;
00088         int count = 0;        
00089         
00090         double norm[4];
00091         
00092         // Get a plane from a random 3 points
00093         if (!GetPlaneFromRnd3(cloud, &norm[0]))
00094             return false;
00095         
00096         // Check to see how many pairs agree with the randomly selected transformation
00097         Vector3 n;
00098         n.x = norm[0]; n.y = norm[1]; n.z = norm[2];
00099         for (unsigned int i = 0; i < cloud->points.size(); i++)
00100         {   
00101             // Get Point as Vector3
00102             Vector3 a1;
00103             a1.x = cloud->points[i].x; a1.y = cloud->points[i].y; a1.z = cloud->points[i].z;
00104             
00105             // Get Distance from point to plane
00106             double mag = n.Dot(a1);
00107             
00108             // Check if distance is less than threshold
00109             if ( fabs(mag-norm[3]) < RANSAC_PLANE_THRESH )
00110                 count++;
00111         }
00112         
00113         // If the current consensus is better than the best, switch the new one in
00114         if ( count > bestcount )
00115         {
00116             if ( !match_coeff || (fabs(norm[0]-normal_coeff[0]) < RANSAC_FLOOR_ANGTHRESH &&
00117                                   fabs(norm[1]-normal_coeff[1]) < RANSAC_FLOOR_ANGTHRESH &&
00118                                   fabs(norm[2]-normal_coeff[2]) < RANSAC_FLOOR_ANGTHRESH &&
00119                                   fabs(norm[3]-normal_coeff[3]) < RANSAC_FLOOR_MAGTHRESH ) ) {
00120                 bestcount = count;
00121                 for (unsigned int r = 0; r < 4; r++) { bestnorm[r] = norm[r]; }
00122             }
00123         }
00124     }
00125 
00126     // Check that the best consensus has atleast RANSAC_PLANE_MINNUM agreements
00127     if (bestcount < RANSAC_PLANE_MINNUM)
00128         return false;
00129 
00130     for (unsigned int r = 0; r < 4; r++) { normal_coeff[r] = bestnorm[r]; }
00131 
00132     return true;
00133 }
00134 
00135 // Perform Least Squares over the inliers of a plane
00136 bool ClearpathDemoTools::PlaneLS(PointCloud* cloud, double* normal_coeff)
00137 {
00138     std::vector<Point> points;
00139 
00140     // Collect all inliers around a ransac'd plane
00141     Vector3 n;
00142     n.x = normal_coeff[0]; n.y = normal_coeff[1]; n.z = normal_coeff[2];
00143     for (unsigned int i = 0; i < cloud->points.size(); i++)
00144     {   
00145         // Get point as Vector3
00146         Vector3 a1;
00147         a1.x = cloud->points[i].x; a1.y = cloud->points[i].y; a1.z = cloud->points[i].z;
00148         
00149         // Get Distance to plane
00150         double mag = n.Dot(a1);
00151         
00152         // Check if distance is less than threshold
00153         if ( fabs(mag-normal_coeff[3]) < RANSAC_PLANE_THRESH )
00154             points.push_back(cloud->points[i]);
00155     }
00156 
00157     // Link to BestFit Library to perform LeastSquares
00158     float normf[4];
00159     getBestFitPlane(points.size(),     // number of input data points
00160                                         &points[0].x,      // starting address of points array.
00161                                         sizeof(points[0]), // stride between input points.
00162                                         0,                 // optional point weighting values.
00163                                         0,                 // weight stride for each vertex.
00164                                         &normf[0]);
00165 
00166     // Make sure normal is facing +y
00167     if (normf[1] < 0.0) {
00168         for (unsigned int r = 0; r < 3; r++) { normal_coeff[r] = -normf[r]; }
00169         normal_coeff[3] = normf[3];
00170     } else {
00171         for (unsigned int r = 0; r < 4; r++) { normal_coeff[r] = normf[r]; }
00172         normal_coeff[3] = -normf[3];
00173     }
00174 }
00175 
00176 void ClearpathDemoTools::PlaneSegment(PointCloud* cloud, PointCloud* plane, PointCloud* seg_cloud, double* norm, double thresh)
00177 {
00178     plane->points.clear();
00179     seg_cloud->points.clear();
00180 
00181     Vector3 n;
00182     n.x = norm[0]; n.y = norm[1]; n.z = norm[2];
00183     for (unsigned int i = 0; i < cloud->points.size(); i++)
00184     {   
00185         Vector3 a1;
00186         a1.x = cloud->points[i].x; a1.y = cloud->points[i].y; a1.z = cloud->points[i].z;
00187         
00188         double mag = n.Dot(a1);
00189         
00190         // Check if distance is less than threshold
00191         if ( fabs(mag-norm[3]) < thresh)
00192             plane->points.push_back(cloud->points[i]);
00193         else
00194             seg_cloud->points.push_back(cloud->points[i]);
00195     }
00196 }
00197 
00198 bool ClearpathDemoTools::TransformByNormal(PointCloud* cloud, PointCloud* cloud_out, double* normal_coeff)
00199 {
00200     cloud_out->points.clear();
00201 
00202     std::vector<Point> points;
00203 
00204     // Check to see how many pairs agree with the randomly selected transformation
00205     Vector3 n, f, r;
00206     n.x = normal_coeff[0]; n.y = normal_coeff[1]; n.z = normal_coeff[2];
00207     f.x = 0.0f; f.y = 0.0f; f.z = 1.0f;
00208     r = n.Cross(f);
00209     f = r.Cross(n);
00210 
00211     for (unsigned int i = 0; i < cloud->points.size(); i++)
00212     {   
00213         Vector3 a1;
00214         a1.x = cloud->points[i].x; a1.y = cloud->points[i].y; a1.z = cloud->points[i].z;
00215         
00216         Point temp = cloud->points[i];
00217         temp.x = r.Dot(a1);
00218         temp.y = n.Dot(a1) - normal_coeff[3];
00219         temp.z = f.Dot(a1);
00220         
00221         cloud_out->points.push_back(temp);
00222     }
00223 }
00224 
00225 // Take a point cloud and return a vertical band of it (y direction)
00226 void ClearpathDemoTools::VerticalCut(PointCloud* cloud, PointCloud* cloud_out, double min, double max, int skim_factor)
00227 {
00228     cloud_out->points.clear();
00229     for (int i = 0; i < cloud->points.size(); i=i+skim_factor)
00230     {
00231         if (cloud->points[i].y > min && cloud->points[i].y < max)
00232         {
00233             cloud_out->points.push_back(cloud->points[i]);
00234         }
00235     }
00236 }
00237 
00238 // Given a position, calculate the velocity command such that it is along a circle with a common ICR
00239 Twist ClearpathDemoTools::DetermineVelocity(double x, double z, double lin_speed)
00240 {
00241     Twist tw;
00242     tw.linear = lin_speed;
00243     tw.angular = 0.0;
00244 
00245     // If X component is essentiall zero, then just go straight
00246     if (fabs(x) < 1e-4)
00247         return tw;
00248 
00249     // Calculate Turning Radius
00250     double r = fabs((x*x + z*z)/(2*x));
00251 
00252     // Calculate Turning Vel
00253     tw.angular = tw.linear/r; // a = r0, v = rw
00254     if (x > 0.0)
00255         tw.angular = -tw.angular;
00256 
00257     return tw;
00258 }
00259 
00260 /******************************************************************
00261 
00262                                  DEMO CODE
00263 
00264 *******************************************************************/
00265 
00266 ClearpathDemoTools::ClearpathDemoTools()
00267 {
00268     mode = MODE_NONE;
00269 }
00270 
00271 ClearpathDemoTools::~ClearpathDemoTools()
00272 {
00273     delete [] init_map;
00274     delete [] mapc;    
00275 }
00276 
00277 // General Initialization
00278 void ClearpathDemoTools::Init(double _lin_speed,
00279                               double _ang_speed,
00280                               double _cam_height,
00281                               double _cam_z_trans,
00282                               unsigned int _numIncX,
00283                               unsigned int _numIncZ)
00284 {
00285     lin_speed = _lin_speed;
00286     ang_speed = _ang_speed;
00287     cam_height = _cam_height;
00288     cam_z_trans = _cam_z_trans;
00289 
00290     minAng = -30.0*3.141592/180.0;
00291     maxAng = 30.0*3.141592/180.0;
00292     drawInc = 4;
00293     obstacleThreshold = 10;
00294 
00295     numIncX = _numIncX;
00296     numIncZ = _numIncZ;
00297     init_map = new Square[numIncX*numIncZ];
00298     mapc = new Square[numIncX*numIncZ];
00299 
00300     cloudCopy.points.clear();
00301     cloudCut.points.clear();
00302     seg.points.clear();
00303     plane.points.clear();
00304     final.points.clear();
00305 
00306     base_normal[0] = 0.0;
00307     base_normal[1] = 1.0;
00308     base_normal[2] = 0.0;
00309     base_normal[3] = cam_height;
00310     last_normal[0] = base_normal[0];
00311     last_normal[1] = base_normal[1];
00312     last_normal[2] = base_normal[2];
00313     last_normal[3] = base_normal[3];
00314 
00315     normal_initialized = false;
00316 }
00317 
00318 // Initialize as the Explore Demo
00319 void ClearpathDemoTools::InitExplore(double _lin_speed,
00320                                      double _ang_speed,
00321                                      double _cam_height,
00322                                      double _cam_z_trans,
00323                                      double _wall_buffer,
00324                                      int _random_walk)
00325 {
00326     mode = MODE_EXPLORE;
00327     Init(_lin_speed, _ang_speed, _cam_height, _cam_z_trans, NUM_INC_ANG, NUM_INC_RNG);
00328 
00329     minX = -30.0*3.141592/180.0;
00330     maxX = 30.0*3.141592/180.0;
00331     minZ = 0.5;
00332     maxZ = 4.0;
00333     incX = (maxX-minX)/double(numIncX);
00334     incZ = (maxZ-minZ)/double(numIncZ);
00335 
00336     random_walk = _random_walk;
00337     wall_buffer = _wall_buffer;
00338     old_clearpath = true;
00339     old_dir = 1.0;
00340 }
00341 
00342 // Initialize as the Person Tracking Demo
00343 void ClearpathDemoTools::InitTrack(double _lin_speed,
00344                                    double _ang_speed,
00345                                    double _cam_height,
00346                                    double _cam_z_trans,
00347                                    double _window_size)
00348 {
00349     mode = MODE_TRACK;
00350     Init(_lin_speed, _ang_speed, _cam_height, _cam_z_trans, NUM_INC_X, NUM_INC_Z);
00351 
00352     minX = -2.0;
00353     maxX = 2.0;
00354     minZ = 0.0;
00355     maxZ = 4.0;
00356     incX = (maxX-minX)/double(numIncX);
00357     incZ = (maxZ-minZ)/double(numIncZ);
00358 
00359     targetX = 0.0;
00360     targetZ = 2.0;
00361     targetW = _window_size;
00362 }
00363 
00364 // Calculate which bin a coordinate lies in
00365 void ClearpathDemoTools::getBins(double x, double z, unsigned int* binX, unsigned int* binZ)
00366 {
00367     *binX = std::min(std::max((int)((x-minX)/incX), 0), int(numIncX)-1);
00368     *binZ = std::min(std::max((int)((z-minZ)/incZ), 0), int(numIncZ)-1);
00369 }
00370 
00371 // Update Interface for Windows (non-ROS) Based Demo
00372 void ClearpathDemoTools::NIUpdate(float* px, float* py, float* pz, unsigned int len, double* return_lin_vel, double* return_ang_vel)
00373 {
00374     PointCloud c, f;
00375     for (int i = 0; i < len; i++)
00376     {
00377         Point p;  p.x = px[i]; p.y = py[i]; p.z = pz[i]; p.rgb = 0.0f;
00378         c.points.push_back(p);
00379     }
00380     Twist tw = this->Update(&c, &f);
00381     *return_lin_vel = tw.linear;
00382     *return_ang_vel = tw.angular;
00383 }
00384 
00385 // Main Update Call
00386 Twist ClearpathDemoTools::Update(PointCloud* cloud, PointCloud* result)
00387 {
00388     // Clear all point clouds
00389     cloudCopy.points.clear();
00390     cloudCut.points.clear();
00391     seg.points.clear();
00392     plane.points.clear();
00393     final.points.clear();
00394 
00395     // Copy Cloud
00396     cloudCopy.points = cloud->points;
00397 
00398     // Initialize Once
00399     if (!normal_initialized)
00400     {
00401         // Setup Initial Map
00402         for (int i = 0; i < numIncX; i++)
00403         {
00404             for (int j = 0; j < numIncZ; j++)
00405             {
00406                 if (mode == MODE_EXPLORE) {
00407                     init_map[i*numIncZ + j].a = minAng + incX*i + incX*0.5;
00408                     init_map[i*numIncZ + j].r = minZ + incZ*j + incZ*0.5;
00409                     init_map[i*numIncZ + j].x = init_map[i*numIncZ + j].r*sin(init_map[i*numIncZ + j].a);
00410                     init_map[i*numIncZ + j].z = init_map[i*numIncZ + j].r*cos(init_map[i*numIncZ + j].a);
00411                     init_map[i*numIncZ + j].c = 0;
00412                 } else if (mode == MODE_TRACK) {
00413                     init_map[i*numIncZ + j].a = 0.0;
00414                     init_map[i*numIncZ + j].r = 0.0;
00415                     init_map[i*numIncZ + j].x = minX + incX*i + incX*0.5;
00416                     init_map[i*numIncZ + j].z = minZ + incZ*j + incZ*0.5;
00417                     init_map[i*numIncZ + j].c = 0;
00418                 }
00419             }
00420         }
00421 
00422         // Guess at Plane Points
00423         VerticalCut(&cloudCopy, &cloudCut, base_normal[3]-0.1, base_normal[3]+0.1, 13);
00424 
00425         // ID Plane
00426         double norm[4];
00427         norm[0] = base_normal[0]; norm[1] = base_normal[1]; norm[2] = base_normal[2]; norm[3] = base_normal[3];
00428         if ( ClearpathDemoTools::PlaneRANSAC(&cloudCut, &norm[0], true) ) {
00429             normal_initialized = true;
00430         } else {
00431             printf("BAD INITIALIZATION, NORMAL FAILED, TRY AGAIN \n");
00432             Twist tw;
00433             return tw;
00434         }
00435 
00436         // Refine Plane
00437         ClearpathDemoTools::PlaneLS(&cloudCut, &norm[0]);
00438         last_normal[0] = norm[0]; last_normal[1] = norm[1]; last_normal[2] = norm[2]; last_normal[3] = norm[3];
00439     }
00440 
00441     // Init Map from stored initial map
00442     memcpy(mapc, init_map, numIncX*numIncZ*sizeof(Square));
00443 
00444     // Segment out floor points
00445     ClearpathDemoTools::PlaneSegment(&cloudCopy, &plane, &seg, &last_normal[0], 0.15);
00446 
00447     // Transform points to be in 2D coord frame
00448     ClearpathDemoTools::TransformByNormal(&seg, &final, &last_normal[0]);
00449 
00450     // Transform poitns to be at robot center
00451     for (int i = 0; i < final.points.size(); i++)
00452     {    
00453         final.points[i].z = final.points[i].z - cam_z_trans;
00454     }
00455 
00456     // Fill Map (Tally how many points are in each bin)
00457     for (int i = 0; i < final.points.size(); i++)
00458     {
00459         double x = final.points[i].x;
00460         double z = final.points[i].z;
00461 
00462         if (mode == MODE_EXPLORE) {
00463             // Explore mode uses a radially based grid
00464             double angle = atan2(x, z);
00465             double range = sqrt(x*x + z*z);
00466             if (angle < minAng || angle > maxAng || range < minZ || range > maxZ)
00467                 continue;
00468             unsigned int binX;
00469             unsigned int binZ;
00470             getBins(angle, range, &binX, &binZ);
00471             unsigned int index = binX*numIncX + binZ;
00472             if (index >= 0 && index < numIncX*numIncZ)
00473                 mapc[binX*numIncZ + binZ].c++;
00474 
00475         } else if (mode == MODE_TRACK) {
00476             // Track mode uses a regular rectilinear grid
00477             unsigned int binX;
00478             unsigned int binZ;
00479             getBins(x, z, &binX, &binZ);
00480             unsigned int index = binX*numIncX + binZ;
00481             if (index >= 0 && index < numIncX*numIncZ)
00482                 mapc[binX*numIncZ + binZ].c++;
00483 
00484         }
00485     }
00486 
00487     // If in exploring mode
00488     if (mode == MODE_EXPLORE) {
00489 
00490         // Add a virtual buffer to all the object edges by adding a object count to all squares within X meters of an occupied square
00491         // TODO Make More Efficient
00492         for (int i = 0; i < numIncX; i++)
00493         {
00494             for (int j = 0; j < numIncZ; j++)
00495             {
00496                 bool colored = false;
00497                 for (int a = 0; a < numIncX; a++)
00498                 {
00499                     for (int b = 0; b < numIncZ; b++)
00500                     {
00501                         if ( mapc[i*numIncZ+j].c == 0 && mapc[a*numIncZ+b].c > 1) {
00502                             double xi = mapc[i*numIncZ+j].x;
00503                             double zi = mapc[i*numIncZ+j].z;
00504                             double xa = mapc[a*numIncZ+b].x;
00505                             double za = mapc[a*numIncZ+b].z;
00506                             double d = sqrt( (xi-xa)*(xi-xa) + (zi-za)*(zi-za) );
00507                             if (d < wall_buffer) {
00508                                 colored = true;
00509                                 mapc[i*numIncZ+j].c = 1;
00510                             }
00511                         }
00512                         if (colored)
00513                             break;
00514                     }
00515                     if (colored)
00516                         break;
00517                 }
00518             }
00519         }
00520 
00521         // We can't see behind any square with an obstacle in it, so we project the obstacle backwards
00522         for (int i = 0; i < numIncX; i++)
00523         {
00524             bool killSlice = false;
00525             for (int j = 0; j < numIncZ; j++)
00526             {
00527                 if (killSlice == true)
00528                     mapc[i*numIncZ+j].c = 1;
00529                 else if (mapc[i*numIncZ+j].c > 0) {
00530                     killSlice = true;
00531                 }
00532             }
00533         }
00534 
00535         // Check to see if there is an obstacle in the first row of squares (we should turn in place to avoid this)
00536         bool clearpath = true;
00537         int dir = 1.0;
00538         if (!old_clearpath)
00539             dir = old_dir;
00540         for (unsigned int i = 0; i < numIncX; i++)
00541         {
00542             if ( mapc[i*numIncZ].c > 0 || mapc[i*numIncZ+1].c > 0)
00543             {
00544                 clearpath = false;
00545                 if ( old_clearpath && double(i) < double(numIncX)/2.0 )
00546                     dir = -1.0;
00547             }
00548         }
00549 
00550         // Update Info about turn (in place) direction
00551         old_clearpath = clearpath;
00552         old_dir = dir;
00553 
00554         // Search for the best square to head towards
00555         double bestxi[NUM_INC_ANG] = {0.0};
00556         double bestzi[NUM_INC_ANG] = {0.0};
00557         double bestd[NUM_INC_ANG] = {0.0};
00558         unsigned int besti[NUM_INC_ANG] = {0};
00559         unsigned int bestOverall = 0;
00560         for (unsigned int i = 0; i < numIncX; i++)
00561         {
00562             for (unsigned int j = 0; j < numIncZ; j++)
00563             {
00564                 if ( mapc[i*numIncZ+j].c == 0)
00565                 {
00566                     double d = mapc[i*numIncZ+j].z ;//sqrt( xi*xi + zi*zi );
00567                     if (d > bestd[bestOverall]) {
00568                         bestOverall = i;
00569                     }
00570                     if (d > bestd[i]) {
00571                         bestxi[i] = mapc[i*numIncZ+j].x;
00572                         bestzi[i] = mapc[i*numIncZ+j].z;
00573                         bestd[i] = d;
00574                         besti[i] = j;
00575                     }
00576                 }
00577             }
00578         }
00579 
00580         if (random_walk > 0)
00581         {
00582             srand ( time(NULL) );
00583             bestOverall = rand() % NUM_INC_ANG;
00584         }
00585 
00586         // Draw Map (Visualization)
00587         for (int i = 0; i < numIncX; i++)
00588         {
00589             for (int j = 0; j < numIncZ; j++)
00590             {
00591                 for (int a = 0; a < drawInc; a++)
00592                 {
00593                     for (int b = 0; b < drawInc; b++)
00594                     {
00595                         Point temp;
00596                         double angle = minAng + incX*i + incX*(double(a)/double(drawInc));
00597                         double range = minZ + incZ*j + incZ*(double(b)/double(drawInc));
00598                         temp.x = range*sin(angle);
00599                         temp.y = 0.0;
00600                         temp.z = range*cos(angle);                    
00601                         char* rgbc = (char*)(&temp.rgb);
00602                         if (mapc[i*numIncZ+j].c == 0) {
00603                             rgbc[0] = 0; rgbc[1] = 255; rgbc[2] = 0;
00604                         } else {
00605                             rgbc[0] = 0; rgbc[1] = 0; rgbc[2] = 255;
00606                         }
00607                         final.points.push_back(temp);
00608                     }
00609                 }
00610             }
00611         }
00612 
00613         // Fill result
00614         result->points = final.points;
00615 
00616         if (!clearpath) {
00617             // Turn in place
00618             Twist tw;
00619             tw.linear = 0.0;
00620             tw.angular = ang_speed*dir;
00621             return tw;
00622         } else if (bestd[bestOverall] > 0.0 && fabs(bestxi[bestOverall]) < 1e-4) {
00623             // Go Straight Forward
00624             Twist tw;
00625             tw.linear = lin_speed;
00626             tw.angular = 0.0;
00627             return tw;
00628         } else if (bestd[bestOverall] > 0.0) {
00629             // Found a target to turn towards
00630             Twist *tw = new Twist[numIncX];
00631             for (unsigned int i = 0; i < numIncX; i++)
00632             {
00633                 tw[i] = ClearpathDemoTools::DetermineVelocity(bestxi[i], bestzi[i], lin_speed);
00634             }
00635             Twist besttw;
00636             besttw.linear = 0.0;
00637             besttw.angular = 0.0;
00638             for (unsigned int i = 0; i < numIncX; i++)
00639             {
00640                 if (copysign(1.0,tw[bestOverall].angular) == copysign(1.0,tw[i].angular))
00641                     if ( fabs(tw[i].angular) > fabs(besttw.angular) )
00642                         besttw = tw[i];
00643             }
00644             delete [] tw;
00645             return besttw;
00646         } else {
00647             // Uh-oh? Don't go anywhere.
00648             Twist tw;
00649             tw.linear = 0.0;
00650             tw.angular = 0.0;
00651             return tw;
00652         }
00653 
00654     } else if (mode == MODE_TRACK) {
00655     
00656         // Find the target square with highest density and head there
00657         // Experimental results show this works pretty well for following people, although there are probably more elegent solutions
00658         double bestx = 0.0;
00659         double bestz = 0.0;
00660         unsigned int bestc = 0;
00661         double bestd = 0.0;
00662         unsigned int bestxbin = 0;
00663         unsigned int bestzbin = 0;
00664         for (unsigned int i = 0; i < numIncX; i++)
00665         {
00666             for (unsigned int j = 0; j < numIncZ; j++)
00667             {
00668                 if (mapc[i*numIncZ+j].c > bestc)
00669                 {
00670                     double x = minX + incX*i + incX*0.5;
00671                     double z = minZ + incZ*j + incZ*0.5;
00672                     double d = sqrt((x-targetX)*(x-targetX) + (z-targetZ)*(z-targetZ));
00673                     if (d < targetW) {
00674                         bestc = mapc[i*numIncZ+j].c;
00675                         bestx = x;
00676                         bestz = z;
00677                         bestxbin = i;
00678                         bestzbin = j;
00679                     }
00680                 }
00681             }
00682         }
00683 
00684         // Set New Target
00685         if (bestc > 0) {
00686             targetX = bestx;
00687             targetZ = bestz;
00688             bestd = sqrt(targetX*targetX + targetZ*targetZ);
00689         }
00690 
00691         // Draw Map (Visualization)
00692         for (int i = 0; i < numIncX; i++)
00693         {
00694             for (int j = 0; j < numIncZ; j++)
00695             {
00696                 for (int a = 0; a < drawInc; a++)
00697                 {
00698                     for (int b = 0; b < drawInc; b++)
00699                     {
00700                         Point temp;
00701                         temp.x = minX + incX*i + incX*(double(a)/double(drawInc));
00702                         temp.y = 0.0;
00703                         temp.z = minZ + incZ*j + incZ*(double(b)/double(drawInc));           
00704                         char* rgbc = (char*)(&temp.rgb);
00705                         if (bestxbin == i && bestzbin == j) {
00706                             rgbc[0] = 255; rgbc[1] = 0; rgbc[2] = 0;
00707                         } else if (mapc[i*numIncZ+j].c == 0) {
00708                             rgbc[0] = 0; rgbc[1] = 255; rgbc[2] = 0;
00709                         } else {
00710                             rgbc[0] = 0; rgbc[1] = 0; rgbc[2] = 255;
00711                         }
00712                         final.points.push_back(temp);
00713                     }
00714                 }
00715             }
00716         }
00717 
00718         // Fill result
00719         result->points = final.points;
00720 
00721         if (bestc > 0) {
00722             // Found a target, move towards it
00723             Twist tw;
00724             if (bestd > 1.0) {
00725                 // Generate twist to move along an arc towards target
00726                 tw = ClearpathDemoTools::DetermineVelocity(targetX, targetZ, lin_speed);
00727             } else {
00728                 // We are closer than 1 meter, simply turn to face target
00729                 tw.linear = 0.0;
00730                 double tempspd = ang_speed*std::min(fabs(bestx/0.35), 1.0);
00731                 if (fabs(bestx) < 0.1)
00732                     tempspd = 0.0;
00733                 tw.angular = -1.0*copysign(tempspd, bestx);
00734             }
00735             return tw;
00736         } else {
00737             // Uh-oh, no target, don't go anywhere
00738             Twist tw;
00739             tw.linear = 0.0;
00740             tw.angular = 0.0;
00741             return tw;       
00742         }
00743     }
00744 
00745 }
00746 
00747 


clearpath_kinect_demo
Author(s): Sean Anderson
autogenerated on Sun Oct 5 2014 22:52:46