Writing world-file, scaffold for monitor.py

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

LinkedListEnumerator.cs

+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;
+        }
+    }
+}

MonitorPyWriter.cs

-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());
         }
     }
 }

Obstacle.cs

+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
+    {
+
+    }
+}

ProcessingException.cs

+using System;
+
+namespace compiler
+{
+    public class ProcessingException : Exception
+    {
+    }
+}

Program.cs

 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);
+                    }
                 }
             }
         }

TestParser.cs

-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();
+            }
         }
     }
 }

WorldWriter.cs

-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();
         }
     }
 }

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>