ARNL performs two major tasks: Localization uses sensor information together with robot odometry data, and a prepared environment map to find the most probable position of the robot within the map, and resets ArRobot's pose to reflect that position within the map's coordinate system. Path Planning and Navigation uses the robot's position and the map to determine a safe route from the current robot position to any given goal position. The path planning task also will drive robot along the planned path, adjusting the path to avoid unmapped obstacles on the way.
There are several ARNL libraries or packages which implement different localization techniques, plus a base library, 'BaseArnl', which includes the path planning task, as well as common features used by the localization libraries. Which localization libraries you have will depend on what software options were purchased. The 'Arnl' localization library contains ArLocalizationTask which uses data obtained from a scanning laser range finder (ArSick) to localize. Arnl is part of the "Laser Navigation" accessory package. The 'SonArnl' localization library contains ArSonarLocalizationTask which uses the sonar (ultrasonic range sensors) for more approximate localization. SonArnl is included with any MobileRobots robot platform and can be used with any MobileRobots robot equipped with sonar. The 'Mogs' localization library contains ArGPSLocalizationTask which uses data from a GPS receiver to correct for odometry error and to locate the robot in a georegistered map. GPS localization is part of the "MOGS Outdoor Navigation" accessory package. It is also possible to use more than one localization tequnique simultaneosly, merging them into a unified result, though this feature is still experimental. All of the localization task classes from the various libraries are derived from ArBaseLocalizationTask in the BaseArnl library.
Path planning and navigation is performed by the ArPathPlanningTask class in the BaseArnl library.
The map (ArMap, in the Aria library) used in localization and planning contains several types of data and objects. Maps may include designated forbidden areas, goals and a "home" poses. You can build maps using the Mapper3 (tm) application from MobileRobots. The two tasks run in background threads. See below for more discussion.
This reference manual describes the SonArnl library, which implements sonar localization in the ArSonarLocalizationTask class. For documentation on the BaseArnl library, see its reference manual in the docs directory.
To write a program using SONARNL, include the Arnl/Aria.h, Arnl/Arnl.h, Arnl/ArSonarLocalizationTask.h, and, if needed, Arnl/ArNetworking.h header files with your source code and link it with libBaseArnl, libAriaForArnl, libAriaNetworkingForArnl, and libSonArnl libraries in Arnl/lib.
You need to include five basic objects to enable localization: ArRobot, ArMap, ArSonarDevice, ArPathPlanningTask, and ArSonarLocalizationTask. Other objects will also be required, such as ArRobotConnector, etc.
For a complete example program using SONARNL see sonarnlServer::cpp
First create an ArSonarLocalizationTask object with an instantiated ArRobot, an ArSonarDevice object and a line map of the robot's environment, explicitly as a file or from ArMap. For example,
// Create the localization task (it will start its own thread here) ArSonarLocalizationTask myLocTask(myRobot, mySonar, mapname);
Next initialize the robot's location inside the map and do an initial localization.
// Set the initial pose to the robot's "Home" position from the map, or // (0,0,0) if none, then let the localization thread take over. myLocTask.localizeRobotAtHomeBlocking();
You can later reset the robot to a unique "home" pose, as prescribed in the map, any time before or after initialization using the localizeRobotAtHomeBlocking() or localizeRobotAtHomeNonBlocking() functions. Or use the forceUpdatePose() method to place the robot in the map:
// Force the current localization pose
myLocTask.forceUpdatePose(pose);
Thereafter, ArSonarLocalizationTask thread automatically acts to relocate the robot in the map. To conserve processing resources, automatic localization updates happen only when the robot has moved beyond set translational and/or rotational limits. By default, these limits are set to +-200 mm translation and +-5 degrees rotation but may be changed by modifying SONARNL's configuration parameters.
Before running your SONARARNL-enabled application, you can first physically position the robot (or simulated robot) at a starting position, such as its home point or at the place corresponding to 0,0,0 in the map so that SONARARNL can sucessfully perform the initial localization at that known point when localizeRobotAtHomeBlocking()
is called. Or, connect with MobileEyes and use its "Localize Robot to Point" command to position the robot in the map and trigger a position reset and re-initialize localization at that new starting point.
#include "Aria/Aria.h" #include "Arnl.h" #include "ArSonarLocalizationTask.h" class MyClass { ArSonarLocalizationTask *myLocTask; ArFunctor1C<MyClass, int> myLocFailedCB; void failed(int n); public: MyClass(ArSonarLocalizationTask *locTask); ~MyClass(); }; MyClass::MyClass(ArSonarLocalizationTask *locTask) : myLocTask(locTask), myLocFailedCB(this, &MyClass::failed) { locTask->addFailedLocalizationCB(&myLocFailedCB); } MyClass::~MyClass() { locTask->remFailedLocalizationCB(&myLocFailedCB); } void MyClass::failed(int n) { ArLog::log(ArLog::Normal, "Localization failure."); }
The mapping process is different for each localization method, though the map files are compatible and can be reused if they contain enough information useful to other methods (for example, laser localization in particular needs an accurate map of laser-sensed obstacles to match with laser readings). To learn more about creating a map for sonar localization with SonArnl, see SonarMapping.txt.