Documentation / v1.0 / Core / Navigation

From emss Framework - iRobot Create C++ Framework

You are here: Documentation / v1.0 / Core / Navigation

---

[edit] Navigation

Files:

Navigation/Navigation.h Licensed under GPLv3 Navigation/Navigation.cpp Licensed under GPLv3

The Navigation class holds the information of the navigation points, or way points, for the robot. Other data structures, such as splines, are also included in this class. Tasks who wish to navigate from point to point or via spline use the Navigation component as an abstraction to do so. The most important methods are calculateLeftRightWheelSpline() and getWheelSpeed(tick,interval) which have been discussed in detail in Section 3.5.1.

  void Navigation::calculateLeftRightWheelSpline() {
        double wheelNodeDistanceBetweenNode = targetSpeed *
                ((double) interval / 1000.0);
        // Init and drop nodes
         wheelLeftSplineX->clearNodes();
         wheelLeftSplineY->clearNodes();
         wheelRightSplineX->clearNodes();
         wheelRightSplineY->clearNodes();
         // Calculate new nodes for wheel splines
         for (int navNode = 0; navNode < navSplineX->getNodeCount() - 1; navNode++) {
                 // Get the distance between the two nav nodes
                 double distanceBetweenNodes =
                        getDistanceBetweenNavNodes(navNode, navNode+1);
                 // Create the wheel nodes at equal distance between the two nodes
                 for (int wheelNode = 0; wheelNode <
                  (distanceBetweenNodes / wheelNodeDistanceBetweenNode); wheelNode++) {
                        // Get the t value for the nav node, which is discretisized
                            based on the wheel node distance between nodes
                        double t = (double) wheelNode / (double)
                                (distanceBetweenNodes / wheelNodeDistanceBetweenNode);
                        // Get the node positions for both the left and right wheel
                        // splines
                        Vector2D c(navSplineX->getValue(navNode, t),
                                navSplineY->getValue(navNode, t));
                        Vector2D c1(navSplineX->getFirstDerivative(navNode, t),
                                navSplineY->getFirstDerivative(navNode, t));
                        Vector2D v = c1 / norm(c1);
                        Vector2D n = Vector2D(-v.y(), v.x());
                        Vector2D l = c + (wheelOffset * n);
                        Vector2D r = c - (wheelOffset * n);
                        wheelLeftSplineX->addNode(l.x());
                        wheelLeftSplineY->addNode(l.y());
                        wheelRightSplineX->addNode(r.x());
                        wheelRightSplineY->addNode(r.y());
                 }
         }
  }

The getWheelSpeed(tick,interval) method requires two parameters: tick and interval. The tick parameter is straightforward and is used to calculate which node in the wheel splines will be used to extract the current wheel speed. The interval parameter is used to insure that the requesting Task has the same interval as used for creating the wheel splines. However, if the setting NAVIGATION_DYNAMIC_INTERVAL_ENABLED is true, getWheelSpeed(tick,interval) checks to see what the actual interval was between the Tasks request for wheel speeds, and, if this interval has drifted too far from the original interval used to create the wheel splines, they are accordingly recalculated. The actual calculation of the wheel speeds are only the last four lines of the getWheelSpeed(tick,interval) method.

  Vector2D Navigation::getWheelSpeed(int tick, int interval) {
         bool rebuildWheelSplines = false;
  int actualInterval = interval;
  // Get the node based on the tick, and the current offset, so that you can
  // start navigation in the middle or so
  int node = tickOffset + tick;
  double t = 0.0;
  //Are we done?
  if (node >= wheelLeftSplineX->getNodeCount() - 1) {
         return SplineNavigationWheelSpeedFinished;
  }
  // Dynamic or fixed interval?
  if(dynamicIntervalEnabled == true) {
         // Dynamic interval - adjust spline for incomming interval but only
         // rebuild if exceeds flexibility
         actualInterval = lastGetWheelSpeedTimestamp.restart();
         // Rebuild spline?
         if(actualInterval > interval + intervalFlexibility ||
                actualInterval < interval - intervalFlexibility) {
                //Debug::print("[Navigation] Interval flexiblity exceeded by
                // %1", interval - actualInterval);
                rebuildWheelSplines = true;
         }
  } else {
         // Fixed interval, nothing to do except warning and return if
         // different
         if (this->interval != interval) {
                return SplineNavigationWheelSpeedFinished;
         }
  }
  // Rebuild wheel splines because of interval delay or node offset?
  if(rebuildWheelSplines == true) {
         interval = actualInterval;
         calculateLeftRightWheelSpline();
  }
  // Calculate speed in mm/s per wheel
  double vl = norm(Vector2D(wheelLeftSplineX->getFirstDerivative(node, t),
         wheelLeftSplineY->getFirstDerivative(node, t)));
  double vr = norm(Vector2D(wheelRightSplineX->getFirstDerivative(node, t),
         wheelRightSplineY->getFirstDerivative(node, t)));
  vl *= 1.0 / (interval / 1000.0);
  vr *= 1.0 / (interval / 1000.0);
  return Vector2D(vl, vr);
}