Fwd.cpp

This file checks Dubins' and FSC paths generation (their 'goto' constructors): a path of each type is built to connect two given configurations, and its characteristics (including starting and final configurations) are given.

// 
// This program verifies the correctness
//  of ISeeML' `goto' constructors for forward-only paths
// 
 
 
// For Borland C++
#ifdef __BORLANDC__
#include <condefs.h>
#endif
 
// To get the definition of ISeeML' forward paths (Dubins & FSC). 
#include <iSeeML/fwdPaths>
 
 
int main() 
{
  //### Constant definitions ###################################
 
  // minimum turning radius (clearer than maximum curvature)
  const double minTurnRadius = 5; 
  // turning distance (dist. to reach min. turn. rad.)
  const double minTurnDist = 5; 
 
 
  //### Configurations definitions #############################
 
  double x, y, th; 
  cout << "Enter the starting configuration as x y th "
       << "(in meters and degres): "; 
  cin  >> x >> y >> th; 
  iSeeML::rob::OrPtConfig start(x, y, M_PI * th / 180);
 
  cout << "Enter the final configuration as x y th (same): "; 
  cin  >> x >> y >> th; 
  iSeeML::rob::OrPtConfig end(x, y, M_PI * th / 180); 
 
  cout << endl; 
 
 
  //### Dubins' paths computations #############################
 
  // maximum curvature is deduced from minimum turning radius
  const double maxCurv = 1 / minTurnRadius; 
 
  // Dubins' path
  iSeeML::rob::DubinsPath DPath(start, end, maxCurv); 
 
  cout << "\nDubins path connecting " << start << " to "
       << end << "\n  is " << DPath << endl; 
 
 
  //### FSC paths computations #################################
 
  // max. curv. derivative is deduced from turning distance
  const double maxCurvDeriv = maxCurv / minTurnDist; 
 
  // Fsc path
  iSeeML::rob::FscPath FPath(start, end, maxCurv, maxCurvDeriv); 
 
  cout << "\nFSC path connecting " << start << " to "
       << end << "\n  is " << FPath << endl << endl; 
 
#ifdef WIN32
  system(pause);
#endif
  return(0); 
} // end of main()