This post is part of the SATF work.
The section elaborates on one particular set of simulation auto acceptance tests – the Monte Carlo localization service tests. These tests are used as an example in the most of the following material, so they are worth of individual scrutiny. By the same token, the MC localization case is complex enough to illustrate the most important aspects of the testing framework including probabilistic testing. Although its implementation part looks Greek at the moment (further sections will clarify it), it is also presented, along with the general description of the tests and problem, to provide the taste of what the whole idea is worth in reality.
The localization problem amounts to determining the pose (two dimensional coordinates and a bearing) of a robot in the world’s frame. Two of its modes will concern us for now: tracking and global localization. In the former one, the initial pose of the robot is known, and the algorithm strives to maintain the further pose estimate. The latter is more ambitious – to determine the actual robot’s pose without any apriori information through the consequent robot motions. The localization algorithm could rely on the robot’s sensors readings, the robot’s motion information and the knowledge of the world’s map. The MC localization algorithm solves both of these tasks but does not provide a 100% success guarantee: it is probabilistic and fails sometimes. As such, it demands employing of some strategy for probabilistic testing.
In all the tests, the success criterion is the same, namely, the pose estimation provided by the localization service should agree with the pose acquired from the physics simulation engine. The first test checks the most basic case, tracking on a straight trajectory: the robot is moving on a straight line for some time from the known initial pose inside environment with the known static map. Along with the stated functionality, the test focuses on correctness of the integration (contributing services will be described below) and the straight line aspect of the motion model, without placing too much stress on other aspects, which are to be tested further. The second test adds a turn to the robot’s trajectory, challenging the rotation logics of the motion model. At that point, since within the tracking setup motion weighs more than measurements in the sense of the algorithm outcome, we could gain some confidence in the motion model implementation. Actually, at the beginning (until not enough drift has been accumulated) tracking could proceed without measurements at all: the algorithm updates the initial pose with the motion information to get a reasonable pose estimate. The third test turns back to the straight motion but does not provide an initial pose estimation, i.e asks for the global localization. Now, the measurement model comes to the fore, supported by the alleged correctness of the motion model. Eliminating inconsistent ones, the MC localization algorithm assesses randomly generated pose guesses (which initially cover the entire pose space) on the basis of sensors’ measurements and the known map. The last test once again adds a turn to the trajectory, which allows to test both models and their interaction to the full extent.
All the actions take place inside a simulated environment resembling some human dwelling without a ceiling: there are walls, a floor, the sky and the sun. The robot is a differential drive platform with a mounted LIDAR sensor. The environment is described in the mc_lrf_localizer.environ.xml file, presenting of which right here will bring me charged with the cruelty to scrollbars, thus only a picture.
MC localization tests environment
The MC localization service depends on other service to fulfil its task. Their composition is described in the manifest file mc_lrf_localizer.manifest.xml. Namely, they are: the odometry service, providing motion data; the simulated laser range finder service, providing range measurements data; the map provider service with the static map; the simulated differential drive service, transmitting motion commands to the robot; and the dashboard service, managing the graphical presentation of the process. Some of them have their own configuration files (mc_lrf_localizer.config.xml, mc_lrf_localizer_diff_drive_odometry.config.xml and mc_lrf_localizer_map_provider.config.xml), also considered as a part of the test manifest.
MC localization tests map
<?xml version="1.0"?>
<!--This file was created with the Microsoft Visual Programming Language.-->
<Manifest xmlns:simulatedreferenceplatform2011="http://brumba.ru/contracts/2013/11/simulatedreferenceplatform2011.html" xmlns:simulatedlrf="http://brumba.ru/contracts/2013/11/simulatedlrf.html" xmlns:this="urn:uuid:de6562ee-97f1-48f7-b4e3-ae1296150cb5" xmlns:dssp="http://schemas.microsoft.com/xw/2004/10/dssp.html" xmlns:simulation="http://schemas.microsoft.com/robotics/2006/04/simulation.html" xmlns:mclrflocalizerservice="http://brumba.ru/contracts/2014/03/mclrflocalizerservice.html" xmlns:dashboard="http://brumba.ru/contracts/2014/11/dashboard.html" xmlns:simulatedlocalizer="http://brumba.ru/contracts/2014/11/simulatedlocalizer.html" xmlns:diffdriveodometryservice="http://brumba.ru/contracts/2013/11/diffdriveodometryservice.html" xmlns:mapproviderservice="http://brumba.ru/contracts/2014/03/mapproviderservice.html" xmlns="http://schemas.microsoft.com/xw/2004/10/manifest.html">
<CreateServiceList>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2013/11/simulatedreferenceplatform2011.html</dssp:Contract>
<dssp:Service>http://localhost:80/stupid_waiter_ref_platform</dssp:Service>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Service>http://localhost/stupid_waiter@</dssp:Service>
<dssp:PartnerList />
<dssp:Name>simulation:Entity</dssp:Name>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:SimulatedReferencePlatformRobot</Name>
</ServiceRecordType>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2013/11/diffdriveodometryservice.html</dssp:Contract>
<dssp:Service>http://localhost:80/odometry@</dssp:Service>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2013/11/diffdriveodometryservice.html</dssp:Contract>
<dssp:Service>mc_lrf_localizer_diff_drive_odometry.config.xml</dssp:Service>
<dssp:PartnerList />
<dssp:Name>dssp:StateService</dssp:Name>
</dssp:Partner>
<dssp:Partner>
<dssp:Contract>http://schemas.microsoft.com/robotics/2006/05/drive.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>diffdriveodometryservice:DifferentialDrive</dssp:Name>
<dssp:ServiceName>this:SimulatedReferencePlatformRobot</dssp:ServiceName>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:BrumbaDifferentialDriveOdometry</Name>
</ServiceRecordType>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2013/11/simulatedlrf.html</dssp:Contract>
<dssp:Service>http://localhost:80/lrf</dssp:Service>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Service>http://localhost/stupid_waiter_lidar</dssp:Service>
<dssp:PartnerList />
<dssp:Name>simulation:Entity</dssp:Name>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:SimulatedLaserRangeFinder</Name>
</ServiceRecordType>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mclrflocalizerservice.html</dssp:Contract>
<dssp:Service>http://localhost:80/localizer@</dssp:Service>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mclrflocalizerservice.html</dssp:Contract>
<dssp:Service>mc_lrf_localizer.config.xml</dssp:Service>
<dssp:PartnerList />
<dssp:Name>dssp:StateService</dssp:Name>
</dssp:Partner>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2013/11/diffdriveodometryservice.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>mclrflocalizerservice:Odometry</dssp:Name>
<dssp:ServiceName>this:BrumbaDifferentialDriveOdometry</dssp:ServiceName>
</dssp:Partner>
<dssp:Partner>
<dssp:Contract>http://schemas.microsoft.com/xw/2005/12/sicklrf.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>mclrflocalizerservice:Lrf</dssp:Name>
<dssp:ServiceName>this:SimulatedLaserRangeFinder</dssp:ServiceName>
</dssp:Partner>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mapproviderservice.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>mclrflocalizerservice:Map</dssp:Name>
<dssp:ServiceName>this:BrumbaMapProvider</dssp:ServiceName>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:BrumbaMonteCarloLocalizer</Name>
</ServiceRecordType>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mapproviderservice.html</dssp:Contract>
<dssp:Service>http://localhost:80/map_provider</dssp:Service>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mapproviderservice.html</dssp:Contract>
<dssp:Service>mc_lrf_localizer_map_provider.config.xml</dssp:Service>
<dssp:PartnerList />
<dssp:Name>dssp:StateService</dssp:Name>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:BrumbaMapProvider</Name>
</ServiceRecordType>
<ServiceRecordType>
<dssp:Contract>http://brumba.ru/contracts/2014/11/dashboard.html</dssp:Contract>
<dssp:PartnerList>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mclrflocalizerservice.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>dashboard:McLrfLocalizer</dssp:Name>
<dssp:ServiceName>this:BrumbaMonteCarloLocalizer</dssp:ServiceName>
</dssp:Partner>
<dssp:Partner>
<dssp:Contract>http://brumba.ru/contracts/2014/03/mapproviderservice.html</dssp:Contract>
<dssp:PartnerList />
<dssp:Name>dashboard:Map</dssp:Name>
<dssp:ServiceName>this:BrumbaMapProvider</dssp:ServiceName>
</dssp:Partner>
</dssp:PartnerList>
<Name>this:BrumbaDashboard</Name>
</ServiceRecordType>
</CreateServiceList>
</Manifest>
The test scenario is stated in code. Unfortunately, it is not too eligible, which is inherent to all MRDS code in general. An outer class in SetUp method prepares service references required by all tests, as well as a listener for notification from the MC localization service. The service employes the notification model (instead of the polling) which is expedient for the testing purposes: each time a localization service pose update notification arrives, the pose from the physics simulation engine is acquired, and the pair is stored until the next notification. Inner classes corresponding to the particular tests configure the localization service (tracking or global localization modes) and issue motion commands to the robot in their Start methods. And then in Test methods after certain time, they compare poses from the localization service and the physics engine within some tolerance.
using System;
using System.Collections.Generic;
using System.Linq;
using Brumba.DsspUtils;
using Brumba.Simulation;
using Brumba.Utils;
using Brumba.SimulationTester;
using MathNet.Numerics;
using Microsoft.Ccr.Core;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.Simulation.Engine.Proxy;
using RefPlPxy = Brumba.Simulation.SimulatedReferencePlatform2011.Proxy;
using DrivePxy = Microsoft.Robotics.Services.Drive.Proxy;
using McLocalizationPxy = Brumba.McLrfLocalizer.Proxy;
using bPose = Brumba.Common.Pose;
using rPose = Microsoft.Robotics.PhysicalModel.Pose;
using BrTimerPxy = Brumba.Entities.Timer.Proxy;
using Vector2 = Microsoft.Xna.Framework.Vector2;
namespace Brumba.SimulationTests
{
[SimTestFixture("mc_lrf_localizer", PhysicsTimeStep = -1, Wip = true)]
public class McLrfLocalizerTests
{
SimulationTesterService TesterService { get; set; }
DrivePxy.DriveOperations RefPlDrivePort { get; set; }
RefPlPxy.ReferencePlatform2011Operations RefPlatformSimulatedPort { get; set; }
McLocalizationPxy.McLrfLocalizerOperations McLrfLocalizationPort { get; set; }
McLocalizationPxy.McLrfLocalizerOperations McLrfLocalizationNotify { get; set; }
public bPose McPose { get; set; }
public bPose SimPose { get; set; }
[SetUp]
public void SetUp(SimulationTesterService testerService)
{
TesterService = testerService;
RefPlDrivePort = testerService.ForwardTo("stupid_waiter_ref_platform/differentialdrive");
McLrfLocalizationPort = testerService.ForwardTo("localizer@");
RefPlatformSimulatedPort = testerService.ForwardTo("stupid_waiter_ref_platform");
McLrfLocalizationNotify = new McLocalizationPxy.McLrfLocalizerOperations();
McLrfLocalizationPort.Subscribe(McLrfLocalizationNotify);
TesterService.Activate(Arbiter.ReceiveWithIterator(false, McLrfLocalizationNotify, GetPoses));
}
IEnumerator GetPoses(McLocalizationPxy.InitPose mcPoseMsg)
{
McPose = (bPose)DssTypeHelper.TransformFromProxy(mcPoseMsg.Body.Pose);
IEnumerable testeeEntitiesPxies = null;
yield return To.Exec(TesterService.GetTesteeEntityProxies, (Action<IEnumerable>)(tep => testeeEntitiesPxies = tep));
SimPose = ((rPose)DssTypeHelper.TransformFromProxy(testeeEntitiesPxies.Single().State.Pose)).SimToMap();
TesterService.Activate(Arbiter.ReceiveWithIterator(false, McLrfLocalizationNotify, GetPoses));
}
void LogResults()
{
TesterService.LogInfo(LogCategory.ActualAndExpectedValues, McPose, SimPose);
TesterService.LogInfo(LogCategory.ActualToExpectedRatio, (McPose.Position - SimPose.Position).Length(), MathHelper2.AngleDifference(McPose.Bearing, SimPose.Bearing));
}
private IEnumerator Wait(float time)
{
var subscribeRq = TesterService.Timer.Subscribe(time);
yield return To.Exec(subscribeRq.ResponsePort);
yield return (subscribeRq.NotificationPort as BrTimerPxy.TimerOperations).P4.Receive();
subscribeRq.NotificationShutdownPort.Post(new Shutdown());
}
[SimTest(7.1f)]
public class Tracking : IStart, ITest, IFixture
{
public McLrfLocalizerTests Fixture { get; set; }
public IEnumerator Start()
{
yield return To.Exec(Fixture.McLrfLocalizationPort.InitPose(new Common.Proxy.Pose(new Vector2(1.7f, 3.25f), 0)));
yield return To.Exec(Fixture.RefPlDrivePort.EnableDrive(true));
//Localization update takes time (~0.3s), if robot keeps moving mclrf position estimate lags for this time (multiplied by velocity)
//Thus in order to get accurate estimate I should stop the robot before assessing test results.
Fixture.TesterService.SpawnIterator(StraightAndStop);
}
private IEnumerator StraightAndStop()
{
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.5, 0.5));
yield return To.Exec(Fixture.Wait, 5f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0, 0));
}
public IEnumerator Test(Action @return, IEnumerable simEntities, double elapsedTime)
{
@return((Fixture.McPose.Position - Fixture.SimPose.Position).Length().AlmostEqualWithError(0, 0.2) &&
MathHelper2.AngleDifference(Fixture.McPose.Bearing, Fixture.SimPose.Bearing).AlmostEqualWithError(0, 0.05));
Fixture.LogResults();
yield break;
}
}
[SimTest(11.1f)]
public class TrackingCurvedPath : IStart, ITest, IFixture
{
public McLrfLocalizerTests Fixture { get; set; }
public IEnumerator Start()
{
yield return To.Exec(Fixture.McLrfLocalizationPort.InitPose(new Common.Proxy.Pose(new Vector2(1.7f, 3.25f), 0)));
yield return To.Exec(Fixture.RefPlDrivePort.EnableDrive(true));
Fixture.TesterService.SpawnIterator(StraightRightStraightControls);
}
IEnumerator StraightRightStraightControls()
{
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.4));
yield return To.Exec(Fixture.Wait, 4.7f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.1));
yield return To.Exec(Fixture.Wait, 1.15f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.4));
yield return To.Exec(Fixture.Wait, 2f);
//Driving out of map, better not to stop
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0, 0));
}
public IEnumerator Test(Action @return, IEnumerable simEntities, double elapsedTime)
{
@return((Fixture.McPose.Position - Fixture.SimPose.Position).Length().AlmostEqualWithError(0, 0.6) &&
MathHelper2.AngleDifference(Fixture.McPose.Bearing, Fixture.SimPose.Bearing).AlmostEqualWithError(0, 0.4));
Fixture.LogResults();
yield break;
}
}
[SimTest(12.1f)]
public class GlobalLocalizationStraightPath : IStart, ITest, IFixture
{
public McLrfLocalizerTests Fixture { get; set; }
public IEnumerator Start()
{
yield return To.Exec(Fixture.McLrfLocalizationPort.InitPoseUnknown());
yield return To.Exec(Fixture.RefPlDrivePort.EnableDrive(true));
Fixture.TesterService.SpawnIterator(StraightAndStop);
}
private IEnumerator StraightAndStop()
{
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.4));
yield return To.Exec(Fixture.Wait, 11f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0, 0));
}
public IEnumerator Test(Action @return, IEnumerable simEntities, double elapsedTime)
{
@return((Fixture.McPose.Position - Fixture.SimPose.Position).Length().AlmostEqualWithError(0, 0.3) &&
MathHelper2.AngleDifference(Fixture.McPose.Bearing, Fixture.SimPose.Bearing).AlmostEqualWithError(0, 0.3));
Fixture.LogResults();
yield break;
}
}
[SimTest(11.1f)]
public class GlobalLocalizationCurvedPath : IStart, ITest, IFixture
{
public McLrfLocalizerTests Fixture { get; set; }
public IEnumerator Start()
{
yield return To.Exec(Fixture.McLrfLocalizationPort.InitPoseUnknown());
yield return To.Exec(Fixture.RefPlDrivePort.EnableDrive(true));
Fixture.TesterService.SpawnIterator(StraightRightStraightControls);
}
public IEnumerator Test(Action @return, IEnumerable simEntities, double elapsedTime)
{
@return((Fixture.McPose.Position - Fixture.SimPose.Position).Length().AlmostEqualWithError(0, 0.6) &&
MathHelper2.AngleDifference(Fixture.McPose.Bearing, Fixture.SimPose.Bearing).AlmostEqualWithError(0, 0.4));
Fixture.LogResults();
yield break;
}
IEnumerator StraightRightStraightControls()
{
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.4));
yield return To.Exec(Fixture.Wait, 4.7f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.1));
yield return To.Exec(Fixture.Wait, 1.15f);
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0.4, 0.4));
yield return To.Exec(Fixture.Wait, 4f);
//Driving out of map, better not to stop
yield return To.Exec(Fixture.RefPlDrivePort.SetDrivePower(0, 0));
}
}
}
}
The single run records of successful tracking and global localization tests are shown below. The intensity of a cell color corresponds to the likelihood of the robot’s pose being in that cell. The single cell with the lime border designates the current pose estimate. In the beginning of the tracking example, the likelihood is spread over the relatively narrow area of the initial guess and could be seen from the outset, in the global localization record on the contrary, the likelihood focuses well enough to be seen only towards the middle of the test. The execution log of multiple test runs with resulting success rates for all mentioned scenarios is also shown.
Mc localization tracking test execution |
Mc localization global localization test execution |
Mc localization tests execution log
The described set of tests for the given problem is by no means exhaustive. It barely checks the key happy paths never touching numerous corner cases. However, these tests pinpoint the backbone functionality in an implementation aware style and treat probabilistic issues integrally. The further testing directions are also clear, like, introducing the artificial drift to the simulation, enlarging of the map, handling the absence of motion, etc. As scarce as they are, the implemented tests allow to develop considerable trust in the software under the question.
| Previous | Next |