From abe8f0c5f11a4bb5c383fec36355240363b347fc Mon Sep 17 00:00:00 2001
From: Nick Le Large <nick.lelarge@kit.edu>
Date: Tue, 24 Sep 2024 10:51:15 +0200
Subject: [PATCH] AStar returns optional to clearly indicate whether path was
 found

---
 demo/include/demo/eat_closest_dot_behavior.hpp |  6 +++---
 demo/include/demo/environment_model.hpp        |  2 +-
 demo/src/astar.cpp                             |  2 +-
 demo/test/astar.cpp                            | 11 +++++++----
 4 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/demo/include/demo/eat_closest_dot_behavior.hpp b/demo/include/demo/eat_closest_dot_behavior.hpp
index c4c5bb2b..0da120dd 100644
--- a/demo/include/demo/eat_closest_dot_behavior.hpp
+++ b/demo/include/demo/eat_closest_dot_behavior.hpp
@@ -22,13 +22,13 @@ class EatClosestDotBehavior : public arbitration_graphs::Behavior<Command> {
 
     Command getCommand(const Time& /*time*/) override {
         auto pacmanPosition = environmentModel_->pacmanPosition();
-        Path pathToClosestDot = environmentModel_->pathToClosestDot(pacmanPosition);
+        std::optional<Path> pathToClosestDot = environmentModel_->pathToClosestDot(pacmanPosition);
 
-        if (pathToClosestDot.empty()) {
+        if (!pathToClosestDot) {
             throw std::runtime_error("Failed to compute path to closest dot. Can not provide a sensible command.");
         }
 
-        return Command{pathToClosestDot};
+        return Command{pathToClosestDot.value()};
     }
 
     bool checkInvocationCondition(const Time& /*time*/) const override {
diff --git a/demo/include/demo/environment_model.hpp b/demo/include/demo/environment_model.hpp
index 89d99c1d..472be24a 100644
--- a/demo/include/demo/environment_model.hpp
+++ b/demo/include/demo/environment_model.hpp
@@ -73,7 +73,7 @@ class EnvironmentModel {
         return astar_.mazeDistance(start, goal);
     }
 
-    Path pathToClosestDot(const Position& position) const {
+    std::optional<Path> pathToClosestDot(const Position& position) const {
         return astar_.pathToClosestDot(position);
     }
 
diff --git a/demo/src/astar.cpp b/demo/src/astar.cpp
index 4721e49f..a4e1d733 100644
--- a/demo/src/astar.cpp
+++ b/demo/src/astar.cpp
@@ -62,7 +62,7 @@ Path AStar::shortestPath(const Position& start, const Position& goal) const {
     return {};
 }
 
-Path AStar::pathToClosestDot(const Position& start) const {
+std::optional<Path> AStar::pathToClosestDot(const Position& start) const {
     // There is a "virtual" position outside of the maze that entities are on when entering the tunnel. We accept a
     // small error in the distance computation by neglecting this and wrapping the position to be on either end of the
     // tunnel.
diff --git a/demo/test/astar.cpp b/demo/test/astar.cpp
index 746000a4..f7d3ecca 100644
--- a/demo/test/astar.cpp
+++ b/demo/test/astar.cpp
@@ -131,15 +131,18 @@ TEST_F(AStarTest, pathToClosestDot) {
     environmentModel_->setMaze({5, 6}, str);
 
     AStar astar(environmentModel_->maze());
-    Path path = astar.pathToClosestDot({1, 2});
+    std::optional<Path> path = astar.pathToClosestDot({1, 2});
+    ASSERT_TRUE(path.has_value());
+
     Path targetPath = {Direction::UP, Direction::RIGHT, Direction::RIGHT};
-    ASSERT_EQ(path.size(), targetPath.size());
+    ASSERT_EQ(path->size(), targetPath.size());
     for (int i = 0; i < targetPath.size(); i++) {
-        EXPECT_EQ(path.at(i), targetPath.at(i));
+        EXPECT_EQ(path->at(i), targetPath.at(i));
     }
 
     path = astar.pathToClosestDot({1, 3});
-    ASSERT_EQ(path.size(), 3);
+    ASSERT_TRUE(path.has_value());
+    ASSERT_EQ(path->size(), 3);
 }
 
 } // namespace utils