Breath first search exploration (#6)

* feature:breath-first-search-exploration
* refactor:destination reached detection
* Add communication
* Update Assets/Scripts/Robot/IRobotController.cs
* Added some comments about the algorithm
* Remove broken collision corrector

Issue #10 for next PR
---------

Co-authored-by: Jakob Meyer Olsen <[email protected]>
Co-authored-by: Puvikaran Santhirasegaram <[email protected]>

Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs

@@ -0,0 +1,117 @@
+// Copyright 2024
+// Contributors: Henneboy
+// This algorithm is not optimal:
+// - Robots can get stuck when they collide with eachother
+// - Robots do not share their targets/intentions, i.e. they don't coordinate.
+//   - Proposed fix: When finding the nearest unexplored tile, the robot could exclude tiles which are close to other robots.
+// - The anti-wall collision 'CollisionCorrector()' is primitive.
+
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using Maes.Map;
+using Maes.Robot;
+using Maes.Robot.Task;
+using UnityEngine;
+
+namespace Maes.ExplorationAlgorithm.HenrikAlgo
+{
+    public class HenrikExplorationAlgorithm : IExplorationAlgorithm
+    {
+        private IRobotController _robotController;
+        private Vector2Int? _targetTile = null;
+        private uint _ticksSinceHeartbeat = 0;
+        public HenrikExplorationAlgorithm()
+        {
+        }
+
+        public HenrikExplorationAlgorithm(Robot2DController robotControllerController)
+        {
+            _robotController = robotControllerController;
+        }
+
+        public void SetController(Robot2DController controller)
+        {
+            this._robotController = controller;
+        }
+
+        public void UpdateLogic()
+        {
+            ShareSlamMap();
+            if (_robotController.GetStatus() == RobotStatus.Idle)
+            {
+                _targetTile = _robotController.GetSlamMap().CoarseMap.GetNearestTileFloodFill(_robotController.GetSlamMap().CoarseMap.GetCurrentPosition(), SlamMap.SlamTileStatus.Unseen);
+            }
+            if (_targetTile != null)
+            {
+                _robotController.PathAndMoveTo(_targetTile.Value);
+                if (_robotController.GetSlamMap().CoarseMap.IsTileExplored(_targetTile.Value))
+                {
+                    _targetTile = null;
+                    _robotController.StopCurrentTask();
+                    return;
+                }
+                return;
+            }
+        }
+
+        private void ShareSlamMap()
+        {
+            if (_ticksSinceHeartbeat == 10)
+            {
+                Debug.Log("Sent slam");
+                var ownHeartbeat = new HeartbeatMessage(_robotController.GetSlamMap());
+                _ticksSinceHeartbeat = 0;
+                _robotController.Broadcast(ownHeartbeat);
+            }
+            var receivedHeartbeat = new Queue<HeartbeatMessage>(_robotController.ReceiveBroadcast().OfType<HeartbeatMessage>());
+            if (receivedHeartbeat.Count > 1)
+            {
+                Debug.Log("Recieved slam");
+                var combinedMessage = receivedHeartbeat.Dequeue();
+                foreach (var message in receivedHeartbeat)
+                {
+                    combinedMessage = combinedMessage.Combine(message);
+                }
+            }
+            _ticksSinceHeartbeat++;
+        }
+
+        public string GetDebugInfo()
+        {
+            var info = new StringBuilder();
+            info.AppendLine($"Target: {_targetTile}.");
+            info.AppendLine($"Current position: {_robotController.GetSlamMap().CoarseMap.GetCurrentPosition()}");
+            info.AppendLine($"Status: {_robotController.GetStatus()}.");
+
+            return info.ToString();
+        }
+    }
+
+    public class HeartbeatMessage
+    {
+        internal SlamMap map;
+
+        public HeartbeatMessage(SlamMap map)
+        {
+            this.map = map;
+        }
+
+        public HeartbeatMessage Combine(HeartbeatMessage otherMessage)
+        {
+            if (otherMessage is HeartbeatMessage heartbeatMessage)
+            {
+                List<SlamMap> maps = new() { heartbeatMessage.map, map };
+                SlamMap.Synchronize(maps); //layers of pass by reference, map in controller is updated with the info from message
+                return this;
+            }
+            return null;
+        }
+
+        public HeartbeatMessage Process() //Combine all, then process, but not really anything to process for heartbeat
+        {
+            return this;
+        }
+    }
+}

Assets/Scripts/HenrikExample.cs

@@ -0,0 +1,188 @@
+// Copyright 2024 MAES
+// 
+// This file is part of MAES
+// 
+// MAES is free software: you can redistribute it and/or modify it under
+// the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or (at your option)
+// any later version.
+// 
+// MAES is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+// Public License for more details.
+// 
+// You should have received a copy of the GNU General Public License along
+// with MAES. If not, see http://www.gnu.org/licenses/.
+// 
+// Contributors: Rasmus Borrisholt Schmidt, Andreas Sebastian Sørensen, Thor Beregaard, Malte Z. Andreasen, Philip I. Holler and Magnus K. Jensen,
+//                      Henrik van Peet & Jakob Meyer Olsen.
+// 
+// Original repository: https://github.com/Molitany/MAES
+
+using System;
+using Maes.ExplorationAlgorithm.Minotaur;
+using System.Collections;
+using Maes.ExplorationAlgorithm.TheNextFrontier;
+using Maes.Map;
+using Maes.Map.MapGen;
+using Maes.Robot;
+using Maes.Utilities.Files;
+using UnityEngine;
+using Maes.Robot;
+using Maes.ExplorationAlgorithm.Movement;
+using System.Collections.Generic;
+using Maes.UI;
+using UnityEditor;
+using System.Linq;
+using Maes.ExplorationAlgorithm.Greed;
+using Maes.ExplorationAlgorithm.RandomBallisticWalk;
+using Maes.ExplorationAlgorithm.HenrikAlgo;
+
+
+namespace Maes
+{
+    internal class HenrikExample : MonoBehaviour
+    {
+        private Simulator _simulator;
+        /*
+*/
+        private void Start()
+        {
+            const int randomSeed = 12345;
+
+            var constraintsDict = new Dictionary<string, RobotConstraints>();
+
+            //var constraintsGlobalCommunication = new RobotConstraints(
+            constraintsDict["Global"] = new RobotConstraints(
+                senseNearbyAgentsRange: 5f,
+                senseNearbyAgentsBlockedByWalls: true,
+                automaticallyUpdateSlam: true,
+                slamUpdateIntervalInTicks: 1,
+                slamSynchronizeIntervalInTicks: 10,
+                slamPositionInaccuracy: 0.2f,
+                distributeSlam: false,
+                environmentTagReadRange: 4.0f,
+                slamRayTraceRange: 7f,
+                relativeMoveSpeed: 1f,
+                agentRelativeSize: 0.6f,
+                calculateSignalTransmissionProbability: (distanceTravelled, distanceThroughWalls) =>
+                {
+                    return true;
+                }
+            );
+
+            //var constraintsMaterials = new RobotConstraints(
+            constraintsDict["Material"] = new RobotConstraints(
+                senseNearbyAgentsRange: 5f,
+                senseNearbyAgentsBlockedByWalls: true,
+                automaticallyUpdateSlam: true,
+                slamUpdateIntervalInTicks: 1,
+                slamSynchronizeIntervalInTicks: 10,
+                slamPositionInaccuracy: 0.2f,
+                distributeSlam: false,
+                environmentTagReadRange: 4.0f,
+                slamRayTraceRange: 7f,
+                relativeMoveSpeed: 1f,
+                agentRelativeSize: 0.6f,
+                materialCommunication: true
+            );
+
+            //var constraintsLOS = new RobotConstraints(
+            constraintsDict["LOS"] = new RobotConstraints(
+                senseNearbyAgentsRange: 5f,
+                senseNearbyAgentsBlockedByWalls: true,
+                automaticallyUpdateSlam: true,
+                slamUpdateIntervalInTicks: 1,
+                slamSynchronizeIntervalInTicks: 10,
+                slamPositionInaccuracy: 0.2f,
+                distributeSlam: false,
+                environmentTagReadRange: 4.0f,
+                slamRayTraceRange: 7f,
+                relativeMoveSpeed: 1f,
+                agentRelativeSize: 0.6f,
+                calculateSignalTransmissionProbability: (distanceTravelled, distanceThroughWalls) =>
+                {
+                    // Blocked by walls
+                    if (distanceThroughWalls > 0)
+                    {
+                        return false;
+                    }
+                    return true;
+                }
+            );
+
+            var simulator = Simulator.GetInstance();
+            var random = new System.Random(1234);
+            List<int> rand_numbers = new List<int>();
+            for (int i = 0; i < 100; i++)
+            {
+                var val = random.Next(0, 1000000);
+                rand_numbers.Add(val);
+            }
+
+            var constraintName = "Global";
+            var robotConstraints = constraintsDict[constraintName];
+            var buildingConfigList100 = new List<BuildingMapConfig>();
+            foreach (int val in rand_numbers)
+            {
+                buildingConfigList100.Add(new BuildingMapConfig(val, widthInTiles: 100, heightInTiles: 100));
+            }
+
+            var constraintIterator = 0;
+            var mapSizes = new List<int> { 50, 75, 100 };
+            var algorithms = new Dictionary<string, RobotSpawner.CreateAlgorithmDelegate>
+                {
+                    { "HenrikExplAlgo", seed => new HenrikExplorationAlgorithm() }
+                };
+            constraintIterator++;
+            var buildingMaps = buildingConfigList100;
+            foreach (var mapConfig in buildingMaps)
+            {
+                var robotCount = 5;
+                foreach (var size in mapSizes)
+                {
+                    foreach (var (algorithmName, algorithm) in algorithms)
+                    {
+
+                        simulator.EnqueueScenario(new SimulationScenario(seed: 123,
+                                                                         mapSpawner: generator => generator.GenerateMap(mapConfig),
+                                                                         robotSpawner: (buildingConfig, spawner) => spawner.SpawnRobotsTogether(
+                                                                             buildingConfig,
+                                                                             seed: 123,
+                                                                             numberOfRobots: robotCount,
+                                                                             suggestedStartingPoint: new Vector2Int(random.Next(0, size), random.Next(0, size)),
+                                                                             createAlgorithmDelegate: algorithm),
+                                                                         statisticsFileName: $"{algorithmName}-seed-{mapConfig.RandomSeed}-size-{size}-comms-{constraintName}-robots-{robotCount}-SpawnTogether",
+                                                                         robotConstraints: robotConstraints)
+                        );
+
+                        var spawningPosList = new List<Vector2Int>();
+                        for (var amountOfSpawns = 0; amountOfSpawns < robotCount; amountOfSpawns++)
+                        {
+                            spawningPosList.Add(new Vector2Int(random.Next(0, size), random.Next(0, size)));
+                        }
+                    }
+                }
+            }
+
+            //Just code to make sure we don't get too many maps of the last one in the experiment
+            var dumpMap = new BuildingMapConfig(-1, widthInTiles: 50, heightInTiles: 50);
+            simulator.EnqueueScenario(new SimulationScenario(seed: 123,
+                mapSpawner: generator => generator.GenerateMap(dumpMap),
+                robotSpawner: (buildingConfig, spawner) => spawner.SpawnRobotsTogether(
+                                                                 buildingConfig,
+                                                                 seed: 123,
+                                                                 numberOfRobots: 5,
+                                                                 suggestedStartingPoint: Vector2Int.zero,
+                                                                 createAlgorithmDelegate: (seed) => new MinotaurAlgorithm(robotConstraints, seed, 2)),
+                statisticsFileName: $"delete-me",
+                robotConstraints: robotConstraints));
+
+            simulator.PressPlayButton(); // Instantly enter play mode
+
+            //simulator.GetSimulationManager().AttemptSetPlayState(SimulationPlayState.FastAsPossible);
+        }
+
+    }
+}