Introduction to Path Planning

ArPathPlanningTask is the navigation component of ARNL. As an asynchronous task (thread) working with the ARIA robot control infrastructure, it's job is to plan the shortest and safest path for and then drive your mobile robot from its current position to any other reachable point in its mapped environment. ArPathPlanningTask navigates to a goal using an ArMap indicating sensable obstacles (walls, furniture, etc.), forbidden lines and areas, goal points, etc. and samples that data into a lower resolution grid. It then searches this grid for cells that are sufficiently far from obstacles to form a clear "global path" for the robot in that grid from the robot's current position to the goal. It then activates an ArAction which drives the robot along that path, and while navigating along this path, ArPathPlanningTask uses a dynamic window method and local replanning to avoid unmapped obstacles. How it plans both the global and local paths may be adjusted by modifying many parameters (see below).

For accurate navigation, also using a localization task is highly recommended.

Example programs using ArPathPlanning and localization are found in the Arnl/examples directory.

ArPathPlanningTask needs an instantiated and connected ArRobot object to hold the robot details, at least one range device (laser rangefinder, sonar, etc.), and ArMap which holds a prepared map of the robot's operating environment (see expanded description of maps in ArLocalization and Arnl docs): Additional range devices can be addedusing the addRangeDevice() function.

  ArPathPlanningTask myPathPlanningTask(myRobot, myLaser, mySonar, myMap);
  

The path planning task is set up as an ArActionGroup in the ARIA scheme of things: Your application sets the goal for the action (ArPose) and a flag to decide if whether or not, once it arrives at the goal, your robot should turn to some particular heading:

Example sets the goal pose to the robot's start up position and heading.
  myPathPlanningTask.pathPlanToPose(ArPose(0,0,0), true);
  

You may also send the robot to a goal and related pose by name, as may be embedded into the ArMap object--"Dock", for example. Simply pass the goal name string to the pathPlanningTask via the pathPlanToGoal function:

Sends the robot to the dock.
  myPathPlanningTask.pathPlanToGoal("Dock");
  

Once the action goal is set, ArPathPlanningTask automatically plans an efficient path and, with ARIA and ArLocalizationTask, drives the robot to the goal.

Four callback functions provide ways for your ARIA application to be notified and respond when the robot arrives at (addGoalDoneCB) or fails to successfully navigate (addGoalFailedCB) to the prescribed goal, if some other application event interrupts navigation (addGoalInterruptedCB), and a generic callback for any state change (addStateChangedCB). Be sure to remove each callback from the class when finished using their respective remGoalDoneCB, remGoalFailedCB, and remGoalInterruptCB methods:

  ArFunctor1<ArPose> goal_done(goalDone); // Define goalDone
  ArFunctor1<ArPose> goal_failed(goalFailed); // goalFailed
  ArFunctor1<ArPose> goal_interrupt(goalInterrupt); // and goalInterrupted handlers.
  myPathPlanningTask.addGoalDoneCB(&goal_done); // Implement each as task callbacks
  myPathPlanningTask.addGoalFailedCB(&goal_failed);
  myPathPlanningTask.addGoalInterruptedCB(&goal_interrupt);
  

Your application may get navigation status with the getState() method, which returns a value from an enumerated list of states (PathPlanningState), or get a textual description of the status using getStatusString or getFailureString (which are equivalent functions, and fill a char* buffer with text describing the current status, including failures).

  ArPathPlanningTask::PathPlanningState s = myPathPlanningTask.getState();
  char[512] text;
  myPathPlanningTask.getStatusString(text, sizeof(text));
  printf("Planning status: %s.\n", text);
  

The path planning program can be customized for specific robots and specific applications through ARIA- and ARNL-specific methods as part of the program, or by modifying the contents of your robot's ARIA or ARNL parameters file, such as params/p3dx.p and params/arnl.p respectively. Versatile parameters include the dimensions and dynamics of obstacle avoidance and path tracking. Others include details about the robot size, the robot's dynamic parameters, including maximum linear and angular accelerations and velocities, and so forth.

Notes: Unlike the localization module, the path planning module physically moves the robot. Though the path planning module can safely navigate through a mapped area in the presence of unknown obstacles, you should be aware of its limitations:

• The robot can only perceive what can be seen by its sensors (laser and sonar). Any obstacle which cannot be sensed will not be avoided even if it is apparent to you. Any areas or boundaries which the robot should not cross such as drop offs in the floor, downstairs, no-go areas etc must be declared forbidden in the map before using it. (For example, table tops may not be sensed and might collide with the top of the robot if the areas below tables are left free)

• The default parameters in arnl.p are meant to drive Pioneer class of robots in office type environments. If you are using a larger and heavier robot such as a PowerBot, remember to alter the default parameters.

• It might help to set the pathplanning grid resolutions to be rational fractions of the robot half width to allow more free space during path planning. Reducing the PlanRes by a factor will increase computation by factor^2. If the computation takes more than the cycle time (default 100ms) robot motion will be affected.

• Note that the accuracy and resolution of the sonar is orders worse than the laser. So in some cases such as in congested areas it might be useful to switch the sonar off by setting the useSonar parameter to false.

• The obstacle avoidance of the robot is designed to avoid obstacles that are relatively slow moving or static and can be sensed. Collision is inevitable if the obstacle rapidly approaches the robot or is not sensed by the robot sensors. Alter the clearance parameters and the speed parameters to suit your application. (Note, MobileRobots is not responsible for any damages or injuries resulting from the use of this software. Always test your software carefully in a controlled environment.)

• When PlanEvertime flag is set to false, the local path planning reuses the old path segment. Due to the smoothing caused by the spline, this path will creep. This flag is meant to be used when there are a lot of unmapped obstacles in the work area and the replanning everytime will cause flip flopping of the path.

• The most important parameters for path planning are MaxSpeed, PlanRes, PlanFreeSpace and CollisionRange.

---