public void Initialize(Robot robot)
{
// Initialize particles
particles = new List<Particle>(configuration.NumberOfParticles);
for (int i = 0; i < configuration.NumberOfParticles; i++)
{
switch (robot.ExpectedZone)
{
case Robot.Zone.Starting:
particles.Add(new Particle(field,
configuration.WallMargin + MathHelper.random.NextDouble() * (ObstacleX - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0));
break;
case Robot.Zone.Mining:
particles.Add(new Particle(field,
DigX + configuration.WallMargin + MathHelper.random.NextDouble() * (DigX - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0));
break;
case Robot.Zone.Unknown:
particles.Add(new Particle(field,
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldWidth - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0));
break;
default:
break;
}
}
// Update tracker
tracker = new Tracker();
tracker.FindCenter(particles);
}
public AutonomyHandler(AutonomyConfiguration configuration, ref TelemetryHandler telemetryHandler)
{
// Assign fields
this.configuration = configuration;
// Create localization logic
localization = new MonteCarloLocalization(configuration);
// Create robot
robot = new Robot(configuration);
// Create stopwatch
stopwatch = new Stopwatch();
// Add telemetry callback
telemetryHandler.TelemetryFeedbackProcessed += telemetryHandler_TelemetryFeedbackProcessed;
// Create timer
timer = new Timer(configuration.Interval);
timer.Elapsed += timer_Elapsed;
// Create algorithms
depositionAlgorithm = new Deposition(configuration);
lowRiskLocalizationAlgorithm = new LowRiskLocalization(configuration);
// Create static state
staticOutput = new Dictionary<CommandFields, short>();
staticOutput[Comms.CommandEncoding.CommandFields.Mode] = 1;
staticOutput[Comms.CommandEncoding.CommandFields.FrontCameraState] = 0;
staticOutput[Comms.CommandEncoding.CommandFields.BucketCameraState] = 0;
staticOutput[Comms.CommandEncoding.CommandFields.RearCameraState] = 0;
}
public void Update(double elapsedTime, Robot robot, lunabotics.RCU.Autonomy.AutonomyHandler.Zone zone, bool useFrontRangeFinder)
{
// Move particles
particles.ForEach(p =>
{
p.SetVelocities(robot.TranslationalVelocity, robot.RotationalVelocity);
p.Move(elapsedTime);
});
// Blast
for (int i = 0; i < configuration.NumberOfParticles * configuration.BlastPercent; i++)
{
double magnitude = configuration.BlastRadius * MathHelper.random.NextDouble();
double angle = MathHelper.random.NextDouble() * 2.0d * Math.PI;
particles[MathHelper.random.Next(configuration.NumberOfParticles)].Set(
tracker.X + magnitude * Math.Cos(angle),
tracker.Y + magnitude * Math.Sin(angle),
MathHelper.random.NextDouble() * 360.0);
}
// Re-initialize
for (int i = 0; i < configuration.NumberOfParticles * configuration.RandomReInitPercent; i++)
{
particles[MathHelper.random.Next(configuration.NumberOfParticles)].Set(
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldWidth - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0);
}
// Kill lost particles
foreach (Particle particle in particles)
{
if (!field.RectangleStarting.Contains((int)particle.X, (int)particle.Y))
{
// Regenerate particle
switch (zone)
{
case AutonomyHandler.Zone.Start:
particle.Set(
configuration.WallMargin + MathHelper.random.NextDouble() * (ObstacleX - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0);
break;
case AutonomyHandler.Zone.Obstacle:
particle.Set(
ObstacleX + configuration.WallMargin + MathHelper.random.NextDouble() * ((DigX - ObstacleX) - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0);
break;
case AutonomyHandler.Zone.Mining:
particle.Set(
DigX + configuration.WallMargin + MathHelper.random.NextDouble() * ((FieldWidth - DigX) - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0);
break;
case AutonomyHandler.Zone.Unknown:
particle.Set(
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldWidth - 2.0 * configuration.WallMargin),
configuration.WallMargin + MathHelper.random.NextDouble() * (FieldHeight - 2.0 * configuration.WallMargin),
MathHelper.random.NextDouble() * 360.0);
break;
default:
break;
}
}
}
// Measure robot sensors
double[] z = robot.ReadSensors(useFrontRangeFinder);
// Initialize weight sum
double weightSum = 0.0;
// Update particle weights
particles.ForEach(p => weightSum += p.UpdateWeight(z, useFrontRangeFinder));
// Store elite
Particle elite = particles.OrderByDescending(p => p.Weight).First();
// Re-weight particles
double sum = 0.0;
particles.ForEach(p =>
{
double normalizedWeight = p.Weight / weightSum;
p.NormalizedWeight = sum + normalizedWeight;
sum += normalizedWeight;
});
// Generate new population
List<Particle> newParticles = new List<Particle>(configuration.NumberOfParticles);
for (int i = 0; i < configuration.NumberOfParticles; i++)
{
// Get random
double random = MathHelper.random.NextDouble();
for (int j = 0; j < configuration.NumberOfParticles; j++)
{
if (random < particles[j].NormalizedWeight)
{
//Debug.WriteLine(random + ":" + j);
newParticles.Add((Particle)particles[j].Clone());
break;
}
}
}
// Put elite back into the population
newParticles[MathHelper.random.Next(configuration.NumberOfParticles)] = (Particle)elite.Clone();
// Calculate confidence
double confSum = 0.0d;
particles.ForEach(p => confSum += p.Weight);
this.confidence = confSum / (double)configuration.NumberOfParticles;
// Overwrite particles
particles = newParticles;
// Update tracker position
tracker.FindCenter(particles);
}
