diff --git a/ConsoleLogger.cs b/ConsoleLogger.cs
index 0e322ca939632d33f1b3e1698b00bf7e98530e48..e0a232174c18ea0d63a24ee2e2a01352bebff703 100644
--- a/ConsoleLogger.cs
+++ b/ConsoleLogger.cs
@@ -10,9 +10,26 @@ namespace compiler
{
if (verbosity <= ConsoleLogger.Verbosity)
{
- Console.WriteLine(msg);
+ var color = ConsoleColor.White;
+ switch (verbosity)
+ {
+ case Verbosity.Debugging: color = ConsoleColor.Gray; break;
+ case Verbosity.Info: color = ConsoleColor.Cyan; break;
+ case Verbosity.Warning: color = ConsoleColor.Yellow; break;
+ case Verbosity.Error: color = ConsoleColor.Red; break;
+ }
+
+ DoInColor(color, () => Console.WriteLine(msg));
}
}
+
+ private static void DoInColor(ConsoleColor color, Action action)
+ {
+ var currentColor = Console.ForegroundColor;
+ Console.ForegroundColor = color;
+ action();
+ Console.ForegroundColor = currentColor;
+ }
}
public enum Verbosity
diff --git a/LinkedListEnumerator.cs b/LinkedListEnumerator.cs
new file mode 100644
index 0000000000000000000000000000000000000000..9c4f3f400b4dd1b775d14ba6626223b18e8b3daa
--- /dev/null
+++ b/LinkedListEnumerator.cs
@@ -0,0 +1,47 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace compiler
+{
+ public class LinkedListEnumerator<T> : IEnumerator<T>
+ {
+ private Func<T, T> Next { get; }
+
+ public LinkedListEnumerator(T element, Func<T, T> next)
+ {
+ this.First = element;
+ this.Next = next;
+ }
+
+ public T First { get; }
+
+ public T Current { get; set; }
+
+ object IEnumerator.Current => throw new NotImplementedException();
+
+ public void Dispose()
+ {
+ GC.SuppressFinalize(this);
+ }
+
+ public bool MoveNext()
+ {
+ if (this.Current == null)
+ {
+ this.Current = this.First;
+ }
+ else
+ {
+ this.Current = this.Next(this.Current);
+ }
+
+ return this.Current != null;
+ }
+
+ public void Reset()
+ {
+ this.Current = this.First;
+ }
+ }
+}
diff --git a/MonitorPyWriter.cs b/MonitorPyWriter.cs
index 73ab9336de5b8b5905fdc5798b859210edb8fc71..6e379ad38c607e8c86ebe4c7408c8a7d661be8a0 100644
--- a/MonitorPyWriter.cs
+++ b/MonitorPyWriter.cs
@@ -1,4 +1,8 @@
-using System.IO;
+using System;
+using System.Globalization;
+using System.IO;
+using System.Linq;
+using System.Text;
namespace compiler
{
@@ -6,13 +10,57 @@ namespace compiler
{
public void Write(string path, TestParser.Node rootNode)
{
- var targetFile = Path.Combine(path, "monitor.py");
- File.WriteAllText(targetFile, $@"
-# ToDo: Add Python Code
+ var sb = new StringBuilder();
+ sb.AppendLine($@"# ToDo: Add Python Code");
+
+ foreach (var node in rootNode)
+ {
+ if (node.Text.StartsWith("constraint:", StringComparison.InvariantCultureIgnoreCase))
+ {
+ sb.AppendLine();
+
+ if (node.Data.Length != 1)
+ {
+ ConsoleLogger.WriteLine("For constraints only 1 t=-Parameter is supportted", Verbosity.Error);
+ throw new ProcessingException();
+ }
+ var time = double.Parse(node.Data[0].Split('=')[1], CultureInfo.InvariantCulture);
+ sb.AppendLine($"# For time t={time:N} check...");
+ var text = node.Text.Substring("constraint:".Length).Trim();
+ var parts = text.Split("in");
+ var valueToEvaluate = parts[0].Trim();
+ var rangeString = parts[1].Trim();
-");
+ var range = rangeString.TrimStart('[').TrimEnd(']').Split(',').Select(x => x.Trim()).ToList();
+ if (range.Count != 2)
+ {
+ ConsoleLogger.WriteLine("Not exact 2 arguments in range", Verbosity.Error);
+ throw new ProcessingException();
+ }
+
+ if (valueToEvaluate.StartsWith("Distance", StringComparison.InvariantCultureIgnoreCase))
+ {
+ var elements = valueToEvaluate.Substring("Distance(".Length).TrimEnd(')').Split(',').Select(x => x.Trim()).ToList();
+ if (elements.Count != 2)
+ {
+ ConsoleLogger.WriteLine("Not exact 2 arguments in 'Distance'-Function", Verbosity.Error);
+ throw new ProcessingException();
+ }
+ sb.AppendLine($"# checking constraint for distance between {elements[0]} and {elements[1]}");
+ sb.AppendLine($"# it should be in range of {range[0]} to {range[1]}");
+ }
+ else
+ {
+ sb.AppendLine($"# checking constraint for value of {valueToEvaluate}");
+ sb.AppendLine($"# it should be in range of {range[0]} to {range[1]}");
+ }
+ }
+ }
+
+ var targetFile = Path.Combine(path, "monitor.py");
+ File.WriteAllText(targetFile, sb.ToString());
}
}
}
diff --git a/Obstacle.cs b/Obstacle.cs
new file mode 100644
index 0000000000000000000000000000000000000000..5ee857d965bbc54ab65aea80ae575c413690860b
--- /dev/null
+++ b/Obstacle.cs
@@ -0,0 +1,36 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+
+namespace compiler
+{
+ public class Obstacle
+ {
+ public Obstacle(string s)
+ {
+ if (!s.StartsWith("Obstacle", StringComparison.InvariantCultureIgnoreCase))
+ {
+ throw new ArgumentException();
+ }
+
+ s = s.Substring("Obstacle".Length).Trim();
+ var splitted = s.Split(' ', 2);
+
+ this.Name = splitted[0];
+ s = splitted[1];
+ s = s.Split('=', 2)[1].Trim();
+ splitted = s.Split('(', 2);
+ this.Type = splitted[0].Trim().ToLower();
+ this.Parameter = splitted[1].TrimEnd(')').Split(',').Select(x => x.Split('=', 2)).ToDictionary(x => x[0].Trim(), x => x[1].Trim());
+ }
+
+ public string Name { get; }
+ public string Type { get; }
+ public Dictionary<string, string> Parameter { get; }
+ }
+
+ public enum ObstacleType
+ {
+
+ }
+}
diff --git a/ProcessingException.cs b/ProcessingException.cs
new file mode 100644
index 0000000000000000000000000000000000000000..c13d6e102e6fbfabcc0310195071af9adc46699d
--- /dev/null
+++ b/ProcessingException.cs
@@ -0,0 +1,8 @@
+using System;
+
+namespace compiler
+{
+ public class ProcessingException : Exception
+ {
+ }
+}
diff --git a/Program.cs b/Program.cs
index 97dfc5ff824e2c6bb5bd5399cf50f7d165cc7856..037e8c502f03b6a1f6fed7d0f152f61f8ed798c3 100644
--- a/Program.cs
+++ b/Program.cs
@@ -1,4 +1,5 @@
using System;
+using System.Globalization;
using System.IO;
using System.Linq;
@@ -9,6 +10,7 @@ namespace compiler
static void Main(string[] args)
{
ConsoleLogger.Verbosity = Verbosity.Debugging;
+ CultureInfo.CurrentCulture = CultureInfo.InvariantCulture;
if (args.Length != 2)
{
@@ -34,10 +36,21 @@ namespace compiler
for (var i = 0; i < rootNodes.Count; i++)
{
- var rootNode = rootNodes[i];
- var path = Path.Combine(targetDirectory, @$"{baseName}-{i}");
- Directory.CreateDirectory(path);
- TestWriter.WriteAll(path, rootNode);
+ try
+ {
+ var rootNode = rootNodes[i];
+ var path = Path.Combine(targetDirectory, @$"{baseName}-{i}");
+ Directory.CreateDirectory(path);
+ TestWriter.WriteAll(path, rootNode);
+ }
+ catch (ProcessingException)
+ {
+ ConsoleLogger.WriteLine("An error occured. See above for details.", Verbosity.Error);
+ }
+ catch (Exception ex)
+ {
+ ConsoleLogger.WriteLine("An unexpected error occured: \n" + ex.Message + "\nStacktrac:" + ex.StackTrace, Verbosity.Error);
+ }
}
}
}
diff --git a/TestParser.cs b/TestParser.cs
index bf99d2633b676616ba99960babbf05513120b6e6..64ef580b246bf6ec78c11a93ef52dca4d9806826 100644
--- a/TestParser.cs
+++ b/TestParser.cs
@@ -1,4 +1,5 @@
-using System.Collections.Generic;
+using System.Collections;
+using System.Collections.Generic;
using System.Linq;
using System.Xml.Linq;
@@ -91,13 +92,23 @@ namespace compiler
}
- public class Node
+ public class Node: IEnumerable<Node>
{
public string Text { get; set; }
public string[] Data { get; set; }
public Node Next { get; set; }
+
+ public IEnumerator<Node> GetEnumerator()
+ {
+ return new LinkedListEnumerator<Node>(this, x => x.Next);
+ }
+
+ IEnumerator IEnumerable.GetEnumerator()
+ {
+ return this.GetEnumerator();
+ }
}
}
}
diff --git a/WorldWriter.cs b/WorldWriter.cs
index f410760f6d6376a25d14750080efdd6741a95fdd..187c39f694031c6cb270b515a7ab705423f3f0d5 100644
--- a/WorldWriter.cs
+++ b/WorldWriter.cs
@@ -1,4 +1,6 @@
-using System.IO;
+using System;
+using System.IO;
+using System.Xml.Linq;
namespace compiler
{
@@ -6,12 +8,187 @@ namespace compiler
{
public void Write(string path, TestParser.Node rootNode)
{
+ var world = CreateBaseWorld();
+
+ bool vehicleFound = false;
+ foreach (var node in rootNode)
+ {
+ if (node.Text.StartsWith("spawn:", StringComparison.CurrentCultureIgnoreCase))
+ {
+ var text = node.Text.Substring("spawn:".Length).Trim();
+ if (text.StartsWith("Vehicle", StringComparison.CurrentCultureIgnoreCase))
+ {
+ if (vehicleFound)
+ {
+ ConsoleLogger.WriteLine("Multiple Vehicles found", Verbosity.Error);
+ throw new ProcessingException();
+ }
+ vehicleFound = true;//spawn the vehicle always at 0 0 0
+ }
+ else if (text.StartsWith("Obstacle", StringComparison.CurrentCultureIgnoreCase))
+ {
+ var obstacle = this.CreateObstacle(new Obstacle(text));
+ world.Add(obstacle);
+ }
+ else
+ {
+ ConsoleLogger.WriteLine("Unknown spawn-Type: " + text, Verbosity.Error);
+ throw new ProcessingException();
+ }
+ }
+ }
+
+ if (!vehicleFound)
+ {
+ ConsoleLogger.WriteLine("No Vehicles found", Verbosity.Error);
+ throw new ProcessingException();
+ }
+
+ var doc = this.CreateDocumentFromWorld(world);
var targetFile = Path.Combine(path, "test.world");
- File.WriteAllText(targetFile, $@"
+ doc.Save(targetFile);
+ }
+
+ private XDocument CreateDocumentFromWorld(XElement world)
+ {
+ var doc = new XDocument(XElement.Parse("<sdf version='1.4'></sdf>"));
+ doc.Root.Add(world);
+ return doc;
+ }
+ private XElement CreateBaseWorld()
+ {
+ return XElement.Parse(@"
+<world name='default'>
+<!-- A global light source -->
+<include>
+ <uri>model://sun</uri>
+</include>
+
+<!-- A ground plane -->
+<include>
+ <uri>model://ground_plane</uri>
+</include>
+
+<physics type='ode'>
+ <real_time_update_rate>1000.0</real_time_update_rate>
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1</real_time_factor>
+ <ode>
+ <solver>
+ <type>quick</type>
+ <iters>150</iters>
+ <precon_iters>0</precon_iters>
+ <sor>1.400000</sor>
+ <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+ </solver>
+ <constraints>
+ <cfm>0.00001</cfm>
+ <erp>0.2</erp>
+ <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+ <contact_surface_layer>0.01000</contact_surface_layer>
+ </constraints>
+ </ode>
+</physics>
+
+<scene>
+ <ambient>0.4 0.4 0.4 1</ambient>
+ <background>0.7 0.7 0.7 1</background>
+ <shadows>true</shadows>
+</scene>
+<gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose>0.0 0.0 17.0 0 1.5708 0</pose>
+ <view_controller>orbit</view_controller>
+ </camera>
+</gui>
+</world>");
+ }
+
+ private XElement CreateObstacle(Obstacle obstacle)
+ {
+ switch (obstacle.Type)
+ {
+ case "box":
+ return XElement.Parse(@$"<model name='obstacle_{obstacle.Name}'>
+<pose frame=''>{obstacle.Parameter["x"]} {obstacle.Parameter["y"]} 0 0 0 0</pose>
+<link name='link'>
+<inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.166667</iyy>
+ <iyz>0</iyz>
+ <izz>0.166667</izz>
+ </inertia>
+</inertial>
+<collision name='collision'>
+ <geometry>
+ <box>
+ <size>{obstacle.Parameter["cx"]} {obstacle.Parameter["cy"]} {obstacle.Parameter["cz"]}</size>
+ </box>
+ </geometry>
+ <max_contacts>10</max_contacts>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <bounce/>
+ <friction>
+ <torsional>
+ <ode/>
+ </torsional>
+ <ode/>
+ </friction>
+ </surface>
+</collision>
+<visual name='visual'>
+ <geometry>
+ <box>
+ <size>{obstacle.Parameter["cx"]} {obstacle.Parameter["cy"]} {obstacle.Parameter["cz"]}</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Grey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+</visual>
+<self_collide>0</self_collide>
+<enable_wind>0</enable_wind>
+<kinematic>0</kinematic>
+</link>
+</model>");
+ case "cylinder":
+ return XElement.Parse($@"<model name='obstacle_1'>
+<pose>{obstacle.Parameter["x"]} {obstacle.Parameter["y"]} 0 0 0 0</pose>
+<link name='link'>
+<collision name='collision'>
+ <geometry>
+ <cylinder>
+ <radius>{obstacle.Parameter["r"]}</radius>
+ <length>{obstacle.Parameter["h"]}</length>
+ </cylinder>
+ </geometry>
+</collision>
+<visual name='visual'>
+ <geometry>
+ <cylinder>
+ <radius>{obstacle.Parameter["r"]}</radius>
+ <length>{obstacle.Parameter["h"]}</length>
+ </cylinder>
+ </geometry>
+</visual>
+</link>
+</model>");
+ }
-");
+ ConsoleLogger.WriteLine("Unknown Obstacle Type '" + obstacle.Type + "'", Verbosity.Error);
+ throw new ProcessingException();
}
}
}
diff --git a/compiler.csproj b/compiler.csproj
index 70f7e771d51d1508362c40ba6db4d0e538478793..d41a57c7cff781687ddf00e71122f0fa24979433 100644
--- a/compiler.csproj
+++ b/compiler.csproj
@@ -16,6 +16,10 @@
<PrivateAssets>all</PrivateAssets>
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
</PackageReference>
+ <PackageReference Include="Microsoft.NetCore.Analyzers" Version="2.9.8">
+ <PrivateAssets>all</PrivateAssets>
+ <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
+ </PackageReference>
</ItemGroup>
</Project>