static void Main(string[] args)
{
LogEvents.Subscribe(i =>
{
i.Operation.Id = "";
Console.WriteLine(i.ToLogString());
}, new[]
{
typeof(PiTop4Board).Assembly,
typeof(FoundationPlate).Assembly,
typeof(ExpansionPlate).Assembly,
typeof(RoverRobot).Assembly,
typeof(StreamingCamera).Assembly,
typeof(Program).Assembly,
});
var js = new XBoxController();
// using ` mjpg_streamer -i "input_uvc.so -d /dev/video0" -o output_http.so`
// ==> http://pi-top.local:8080/?action=stream
PiTop4Board.Instance.UseCamera();
using var rover = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0),
RoverRobotConfiguration.Default);
var camControl = rover.TiltController;
var motorControl = rover.MotionComponent as SteeringMotorController;
rover.AllLightsOn();
rover.BlinkAllLights();
Observable.Interval(TimeSpan.FromMilliseconds(10))
.Select(_ => (X: js.LeftStick.X, Y: js.LeftStick.Y))
.DistinctUntilChanged()
.Subscribe(stick =>
{
var left = stick.X.WithDeadZone(-.5, .5, .3);
var forward = stick.Y;
motorControl.SetPower((forward + left) / 1.5, (forward - left) / 1.5);
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.A)
.Subscribe(e =>
{
if (e.Pressed)
{
rover.AllLightsOn();
}
else
{
rover.AllLightsOff();
}
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.X && e.Pressed)
.Subscribe(e =>
{
rover.Camera.GetFrame().Save("/home/pi/shot.jpg");
});
Observable.Interval(TimeSpan.FromMilliseconds(100))
.Select(_ => (X: js.RightStick.X, Y: js.RightStick.Y))
.DistinctUntilChanged()
.Subscribe(stick =>
{
camControl.SetSpeeds(
RotationalSpeed.FromRadiansPerSecond(stick.X / 3),
RotationalSpeed.FromRadiansPerSecond(stick.Y / 3)
);
});
js.Events.OfType <ButtonEvent>().Subscribe(e => Console.WriteLine($"Button: {e.Button} {e.Pressed}"));
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.RightStick)
.Subscribe(e =>
{
camControl.Reset();
});
//Observable.Interval(TimeSpan.FromMilliseconds(100))
// .Select(_ =>
// new Unit[10].Select(_ => rover.UltrasoundFront.Distance.Centimeters).ToArray())
// .Subscribe(l =>
// {
// var mean = l.Average();
// var stddev = Math.Sqrt(l.Select(d => (d - mean) * (d - mean)).Average());
// var maxrange = 1.5 * stddev;
// var valid = l.Where(d => Math.Abs(d - mean) < maxrange).ToList();
// if (valid.Count > 0)
// {
// Console.WriteLine($"Distance= {valid.Average():F1} cm ({valid.Count}: mean {mean:F1}, stddev {stddev:F1}, max {valid.Max():F1}, min {valid.Min():F1})");
// }
// });
var oef = new OneEuroFilter(minCutoff: 0.004, beta: 0.7);
Observable.Interval(TimeSpan.FromMilliseconds(100)).Select(_ => oef.Apply(rover.UltrasoundFront.Distance.Centimeters))
.Scan(new List <double>(), (list, d) => // sliding window
{
const int window = 10;
list.Add(d);
if (list.Count > window)
{
list.RemoveRange(0, list.Count - window);
}
return(new List <double>(list));
})
.Subscribe(l =>
{
var mean = l.Average();
var stddev = Math.Sqrt(l.Select(d => (d - mean) * (d - mean)).Average());
var maxrange = 1.5 * stddev;
var valid = l.Where(d => Math.Abs(d - mean) < maxrange).ToList();
if (valid.Count > 0)
{
Console.WriteLine($"Distance= {valid.Average():F1} cm ({valid.Count}: mean {mean:F1}, stddev {stddev:F1}, max {valid.Max():F1}, min {valid.Min():F1})");
}
});
Console.WriteLine("Ok, go drive around");
Console.ReadKey();
rover.AllLightsOff();
rover.Dispose();
}
static void Main(string[] args)
{
LogEvents.Subscribe(i => Console.WriteLine(i.ToLogString()), new[]
{
typeof(PiTop4Board).Assembly,
typeof(FoundationPlate).Assembly,
typeof(ExpansionPlate).Assembly,
typeof(RoverRobot).Assembly,
});
Console.WriteLine("Test Rover App");
PiTop4Board.Instance.UseCamera();
using var rover = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <OpenCvCamera>(0), RoverRobotConfiguration.Default);
var camControl = rover.TiltController;
var motorControl = rover.MotionComponent as SteeringMotorController;
var js = new LinuxJoystick();
rover.AllLightsOn();
rover.BlinkAllLights();
Console.WriteLine("reset");
while (!Console.KeyAvailable)
{
try
{
var e = js.ReadEvent();
// Console.WriteLine($"ts={e.timestamp}, v={e.value}, t={e.type}, n={e.number}");
if (e.type == 1)
{
switch (e.number)
{
case 0:
if (e.value > 0)
{
rover.AllLightsOn();
}
else
{
rover.AllLightsOff();
}
break;
}
}
if (e.type == 2) // axis
{
switch (e.number)
{
case 0: // steer
motorControl.Steering = RotationalSpeed.FromDegreesPerSecond(
e.value.Interpolate(-motorControl.MaxSteering.DegreesPerSecond,
motorControl.MaxSteering.DegreesPerSecond) / 2);
break;
case 1: // throttle
motorControl.Speed = Speed.FromMetersPerSecond(
e.value.Interpolate(motorControl.MaxSpeed.MetersPerSecond,
-motorControl.MaxSpeed.MetersPerSecond) / 2);
break;
case 2: // pan
camControl.Pan = Angle.FromDegrees(e.value.Interpolate(90, -90));
break;
case 3: // tilt
camControl.Tilt = Angle.FromDegrees(
Math.Min(45, e.value.Interpolate(90, -90)));
break;
}
}
}
catch
{
motorControl.Stop();
throw;
}
}
Console.ReadKey();
rover.AllLightsOff();
Console.WriteLine("bye");
}
static void Main(string[] args)
{
//LogEvents.Subscribe(i =>
//{
// i.Operation.Id = "";
// Console.WriteLine(i.ToLogString());
//}, new[]
//{
// typeof(PiTop4Board).Assembly,
// typeof(FoundationPlate).Assembly,
// typeof(ExpansionPlate).Assembly,
// typeof(RoverRobot).Assembly,
// typeof(StreamingCamera).Assembly,
// typeof(Program).Assembly,
//});
ImageClassifier.Register("onnx", () => new OnnxImageClassifier());
ImageClassifier classifier = null;
var js = new XBoxController();
// using ` mjpg_streamer -i "input_uvc.so -d /dev/video0" -o output_http.so`
// ==> http://pi-top.local:8080/?action=stream
PiTop4Board.Instance.UseCamera();
using var rover = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(), PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0),
RoverRobotConfiguration.Default);
var camControl = rover.TiltController;
var motorControl = rover.MotionComponent as SteeringMotorController;
rover.AllLightsOn();
rover.BlinkAllLights();
Observable.Interval(TimeSpan.FromMilliseconds(10))
.Select(_ => (X: js.LeftStick.X, Y: js.LeftStick.Y))
.DistinctUntilChanged()
.Subscribe(stick =>
{
var left = stick.X.WithDeadZone(-.5, .5, .3);
var forward = stick.Y;
motorControl.SetPower((forward + left) / 1.5, (forward - left) / 1.5);
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.A)
.Subscribe(e =>
{
if (e.Pressed)
{
rover.AllLightsOn();
}
else
{
rover.AllLightsOff();
}
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.B && e.Pressed)
.Subscribe(e =>
{
rover.Camera.GetFrame().Save("/home/pi/shot.jpg");
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.Y && e.Pressed)
.Subscribe(e =>
{
classifier?.Dispose();
var signatureFile = new FileInfo("/home/pi/models/pics/signature.json");
classifier = ImageClassifier.CreateFromSignatureFile(signatureFile);
Console.WriteLine($"Loaded model from {signatureFile.FullName}");
});
js.Events.OfType <ButtonEvent>().Where(e => e.Button == Button.X && e.Pressed)
.Subscribe(e =>
{
var frame = rover.Camera.GetFrame().Focus();
var result = classifier?.Classify(frame.CloneAs <Rgb24>());
if (result is { })
{
Console.WriteLine($"{result.Prediction.Label}");
}
});
private static async Task ConfigureRover(CSharpKernel csharpKernel)
{
Microsoft.DotNet.Interactive.Formatting.Formatter.ListExpansionLimit = 42;
using var _ = Log.OnEnterAndExit();
await LoadAssemblyAndAddNamespace <RoverRobot>(csharpKernel);
await LoadAssemblyAndAddNamespace <ResourceScanner>(csharpKernel);
await AddNamespace(csharpKernel, typeof(ImageProcessing.ImageExtensions));
var RoverBody = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(),
PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0),
RoverRobotConfiguration.Default);
var RoverBrain = new RoverRobotAgent();
var ResourceScanner = new ResourceScanner();
RoverBody.BlinkAllLights();
await csharpKernel.SetVariableAsync(nameof(RoverBody), RoverBody);
await csharpKernel.SetVariableAsync(nameof(RoverBrain), RoverBrain);
await csharpKernel.SetVariableAsync(nameof(ResourceScanner), ResourceScanner);
var command = new Command("#!reset", "Reset RoverBody, RoverBrain and BrainState")
{
new Option <bool>("--body", description: "Resets the rover body"),
new Option <bool>("--brain", description: "Resets the rover brain"),
new Option <bool>("--state", description: "Resets the rover brain state"),
new Option <bool>("--all", description: "Resets the entire rover"),
};
command.Handler = CommandHandler.Create <bool, bool, bool, bool, KernelInvocationContext>(async(body, brain, state, all, context) =>
{
if (body || brain || state || all)
{
var code = new StringBuilder();
var resetTarget = new List <string>();
if (brain || all)
{
code.AppendLine($"{nameof(RoverBrain)}.Reset();");
resetTarget.Add("brain");
}
if (state || all)
{
code.AppendLine($"{nameof(RoverBrain)}.ClearState();");
resetTarget.Add("state");
}
if (body || all)
{
code.AppendLine($"{nameof(RoverBody)}.Reset();");
resetTarget.Add("body");
}
var value = context.Display($"Reset for {string.Join(", ", resetTarget)} in progress", PlainTextFormatter.MimeType);
await csharpKernel.SendAsync(new SubmitCode(code.ToString()));
value.Update($"Reset for {string.Join(", ", resetTarget)} done!");
}
});
csharpKernel.AddDirective(command);
var source = new CancellationTokenSource();
var robotLoop = Task.Run(() =>
{
using var operation = Log.OnEnterAndExit("roverBrainLoop");
while (!source.IsCancellationRequested)
{
if (!source.IsCancellationRequested)
{
using var __ = operation.OnEnterAndExit("Perceive");
try
{
RoverBrain.Perceive();
}
catch (Exception e)
{
__.Error(e);
}
}
if (!source.IsCancellationRequested)
{
var planResult = PlanningResult.NoPlan;
using var ___ = operation.OnEnterAndExit("Plan");
try
{
planResult = RoverBrain.Plan();
}
catch (Exception e)
{
___.Error(e);
planResult = PlanningResult.NoPlan;
}
if (!source.IsCancellationRequested && planResult != PlanningResult.NoPlan)
{
using var ____ = operation.OnEnterAndExit("Act");
RoverBrain.Act();
}
}
}
RoverBody.MotionComponent.Stop();
}, source.Token);
var reactLoop = Task.Run(() =>
{
using var operation = Log.OnEnterAndExit("roverBrainReactLoop");
while (!source.IsCancellationRequested)
{
if (!source.IsCancellationRequested)
{
using var __ = operation.OnEnterAndExit("React");
try
{
RoverBrain.React();
}
catch (Exception e)
{
__.Error(e);
}
}
}
RoverBody.MotionComponent.Stop();
}, source.Token);
csharpKernel.RegisterForDisposal(() =>
{
source.Cancel(false);
Task.WaitAll(new[] { robotLoop, reactLoop }, TimeSpan.FromSeconds(10));
RoverBody.Dispose();
});
}
private static async Task ConfigureRover(CSharpKernel csharpKernel)
{
using var _ = Log.OnEnterAndExit();
await LoadAssemblyAndAddNamespace <RoverRobot>(csharpKernel);
await LoadAssemblyAndAddNamespace <ResourceScanner>(csharpKernel);
await AddNamespace(csharpKernel, typeof(ImageProcessing.ImageExtensions));
var roverBody = new RoverRobot(PiTop4Board.Instance.GetOrCreateExpansionPlate(),
PiTop4Board.Instance.GetOrCreateCamera <StreamingCamera>(0),
RoverRobotConfiguration.Default);
var roverBrain = new RoverRobotStrategies();
var resourceScanner = new ResourceScanner();
roverBody.BlinkAllLights();
await csharpKernel.SetVariableAsync(nameof(roverBody), roverBody);
await csharpKernel.SetVariableAsync(nameof(roverBrain), roverBrain);
await csharpKernel.SetVariableAsync(nameof(resourceScanner), resourceScanner);
var source = new CancellationTokenSource();
var robotLoop = Task.Run(() =>
{
using var operation = Log.OnEnterAndExit("roverBrainLoop");
while (!source.IsCancellationRequested)
{
var localCancellationSource = new CancellationTokenSource();
if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested)
{
using var __ = operation.OnEnterAndExit("Perceive");
try
{
roverBrain.Perceive?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token);
}
catch (Exception e)
{
__.Error(e);
}
}
if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested)
{
var planResult = PlanningResult.NoPlan;
using var ___ = operation.OnEnterAndExit("Plan");
try
{
planResult = roverBrain.Plan?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token) ??
PlanningResult.NoPlan;
}
catch (Exception e)
{
___.Error(e);
planResult = PlanningResult.NoPlan;
}
if (!source.IsCancellationRequested && planResult != PlanningResult.NoPlan && !localCancellationSource.IsCancellationRequested)
{
using var ____ = operation.OnEnterAndExit("Act");
roverBrain.Act?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token);
}
}
}
roverBody.MotionComponent.Stop();
}, source.Token);
var reactLoop = Task.Run(() =>
{
using var operation = Log.OnEnterAndExit("roverBrainReactLoop");
while (!source.IsCancellationRequested)
{
var localCancellationSource = new CancellationTokenSource();
if (!source.IsCancellationRequested && !localCancellationSource.IsCancellationRequested)
{
using var __ = operation.OnEnterAndExit("React");
try
{
roverBrain.React?.Invoke(roverBody, DateTime.Now, localCancellationSource.Token);
}
catch (Exception e)
{
__.Error(e);
}
}
}
roverBody.MotionComponent.Stop();
}, source.Token);
csharpKernel.RegisterForDisposal(() =>
{
source.Cancel(false);
Task.WaitAll(new[] { robotLoop, reactLoop }, TimeSpan.FromSeconds(10));
roverBody.Dispose();
});
}
