Port tests from di example

functional_programming_tests/include/pathing/utilities.hpp

@@ -27,4 +27,12 @@ std::optional<Map<unsigned char>> parseSetMapRequest(
  */
 std_msgs::msg::UInt8MultiArray createUInt8MultiArrayMessageFromPath(
     Path const& path);
+
+/**
+ * @brief Convert a path from ROS message to a vector of positions
+ * @param msg The message to convert
+ * @return The path as a vector of positions
+ */
+std::vector<Position> parseGeneratedPath(
+    const std_msgs::msg::UInt8MultiArray& msg);
 }  // namespace pathing::utilities

functional_programming_tests/src/utilities.cpp

@@ -64,4 +64,23 @@ std_msgs::msg::UInt8MultiArray createUInt8MultiArrayMessageFromPath(
   }
   return message;
 }
+
+std::vector<Position> parseGeneratedPath(
+    const std_msgs::msg::UInt8MultiArray& msg) {
+  // Check the path has an even number of elements
+  if ((msg.data.size() % 2) != 0) {
+    return {};
+  }
+  if (msg.data.size() == 0) {
+    return {};
+  }
+  Path path;
+
+  for (size_t idx = 0; idx < (msg.data.size() - 1); idx += 2) {
+    path.push_back({msg.data[idx], msg.data[idx + 1]});
+  }
+
+  return path;
+}
+
 };  // namespace pathing::utilities

functional_programming_tests/test/with_functional_programming.cpp

@@ -336,6 +336,201 @@ TEST(GeneratePath, PathGenerated) {
   // THEN there should no errors
   EXPECT_TRUE(response.has_value());
 }
+
+/**
+ * @brief Create a sample occupancy map in a request message
+ * @return A shared pointer to the request message with the occupancy map
+ */
+std::shared_ptr<example_srvs::srv::SetMap::Request> make_occupancy_map() {
+  auto const request = std::make_shared<example_srvs::srv::SetMap::Request>();
+
+  request->map = std_msgs::msg::UInt8MultiArray();
+
+  request->map.layout.dim.resize(3, std_msgs::msg::MultiArrayDimension());
+
+  request->map.layout.dim[0].label = "rows";
+  request->map.layout.dim[0].size = 8;
+  request->map.layout.dim[0].stride = 64;
+
+  request->map.layout.dim[1].label = "columns";
+  request->map.layout.dim[1].size = 8;
+  request->map.layout.dim[1].stride = 1;
+
+  request->map.layout.dim[2].label = "channel";
+  request->map.layout.dim[2].size = 1;
+  request->map.layout.dim[2].stride = 1;
+
+  request->map.data = {0, 0, 0, 0, 0, 0, 0, 0,  //
+                       0, 0, 0, 1, 0, 0, 0, 0,  //
+                       0, 0, 0, 1, 0, 0, 0, 0,  //
+                       0, 0, 1, 1, 1, 0, 0, 0,  //
+                       0, 0, 1, 0, 1, 1, 0, 0,  //
+                       0, 0, 1, 0, 0, 0, 0, 0,  //
+                       0, 0, 0, 0, 0, 0, 0, 0,  //
+                       0, 0, 0, 0, 0, 0, 0, 0};
+  return request;
+}
+
+// Create mock middleware handle
+struct MockMiddlewareHandle : public pathing::Manager::MiddlewareHandle {
+  MOCK_METHOD(void, register_set_map_service, (SetMapCallback), (override));
+  MOCK_METHOD(void, register_generate_path_service, (GeneratePathCallback),
+              (override));
+  MOCK_METHOD(void, log_info, (std::string const&), (override));
+  MOCK_METHOD(void, log_error, (std::string const&), (override));
+};
+
+TEST(PathManager, Ctor) {
+  // GIVEN a mock middleware handle
+  auto mw = std::make_unique<MockMiddlewareHandle>();
+
+  // THEN it should register the services
+  EXPECT_CALL(*mw, register_set_map_service(testing::_)).Times(1);
+  EXPECT_CALL(*mw, register_generate_path_service(testing::_)).Times(1);
+
+  // WHEN the path generator is constructed
+  auto const path_generator = pathing::Manager{std::move(mw)};
+}
+
+TEST(PathManager, SetMap) {
+  // GIVEN a path generator
+  auto mw = std::make_unique<MockMiddlewareHandle>();
+  // Capture the callback so it can be called later
+  pathing::Manager::MiddlewareHandle::SetMapCallback callback;
+  ON_CALL(*mw, register_set_map_service(testing::_))
+      .WillByDefault(testing::SaveArg<0>(&callback));
+
+  auto const path_generator = pathing::Manager{std::move(mw)};
+
+  // WHEN the set map service is called
+  auto const request = make_occupancy_map();
+  auto response = std::make_shared<example_srvs::srv::SetMap::Response>();
+  callback(request, response);
+
+  // THEN the path generator should successfully set the map
+  EXPECT_TRUE(response->success.data);
+}
+
+TEST(PathManager, NoCostmap) {
+  // GIVEN a path generator with a costmap
+  auto mw = std::make_unique<MockMiddlewareHandle>();
+  // Capture the path callback so it can be called later
+  pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback;
+  ON_CALL(*mw, register_generate_path_service(testing::_))
+      .WillByDefault(testing::SaveArg<0>(&path_callback));
+
+  auto const path_generator = pathing::Manager{std::move(mw)};
+
+  // WHEN the generate path service is called without a costmap
+  auto path_request = std::make_shared<example_srvs::srv::GetPath::Request>();
+  path_request->start.data = {0, 0};
+  path_request->goal.data = {0, 0};
+
+  auto path_response = std::make_shared<example_srvs::srv::GetPath::Response>();
+  path_callback(path_request, path_response);
+
+  // THEN the path generator should fail
+  EXPECT_FALSE(path_response->success.data);
+}
+
+struct PathManagerFixture : public testing::Test {
+  /**
+   * @brief Construct a fixture which will set the map and capture the path
+   * callback
+   */
+  PathManagerFixture() : mw_{std::make_unique<MockMiddlewareHandle>()} {
+    // When the map callback is called, set the costmap
+    ON_CALL(*mw_, register_set_map_service(testing::_))
+        .WillByDefault([&](auto const& map_callback) {
+          auto const map_request = make_occupancy_map();
+          auto map_response =
+              std::make_shared<example_srvs::srv::SetMap::Response>();
+          map_callback(map_request, map_response);
+        });
+
+    // Capture the path callback so it can be called later
+    ON_CALL(*mw_, register_generate_path_service(testing::_))
+        .WillByDefault(testing::SaveArg<0>(&path_callback_));
+  }
+
+  std::unique_ptr<MockMiddlewareHandle> mw_;
+  pathing::Manager::MiddlewareHandle::GeneratePathCallback path_callback_;
+};
+
+TEST_F(PathManagerFixture, NoStartNoGoal) {
+  // GIVEN a path generator with a costmap
+  auto const path_generator = pathing::Manager{std::move(mw_)};
+
+  // WHEN the generate path service is called,
+  auto const path_request =
+      std::make_shared<example_srvs::srv::GetPath::Request>();
+  auto path_response = std::make_shared<example_srvs::srv::GetPath::Response>();
+  path_callback_(path_request, path_response);
+
+  // THEN the path generator should fail
+  EXPECT_FALSE(path_response->success.data);
+}
+
+TEST_F(PathManagerFixture, SameStartGoal) {
+  // GIVEN a path generator with a costmap
+  auto const path_generator = pathing::Manager{std::move(mw_)};
+
+  // WHEN the generate path service is called with the same start and goal
+  auto path_request = std::make_shared<example_srvs::srv::GetPath::Request>();
+  path_request->start.data = {0, 0};
+  path_request->goal.data = {0, 0};
+  auto path_response = std::make_shared<example_srvs::srv::GetPath::Response>();
+  path_callback_(path_request, path_response);
+
+  // THEN the path generator should succeed
+  EXPECT_TRUE(path_response->success.data);
+  auto const expected = pathing::Path{{0, 0}};
+  // AND the path should be the same as the start
+  EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
+            expected);
+}
+
+TEST_F(PathManagerFixture, NoPath) {
+  // GIVEN a path generator with a costmap
+  auto const path_generator = pathing::Manager{std::move(mw_)};
+
+  // WHEN the generate path service is called with an unreachable goal
+  auto path_request = std::make_shared<example_srvs::srv::GetPath::Request>();
+  path_request->start.data = {2, 2};
+  path_request->goal.data = {5, 5};
+
+  auto path_response = std::make_shared<example_srvs::srv::GetPath::Response>();
+  path_callback_(path_request, path_response);
+
+  // THEN the path generator should succeed
+  EXPECT_FALSE(path_response->success.data);
+  auto const expected = pathing::Path{};
+  // AND the path should be empty
+  EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
+            expected);
+}
+
+TEST_F(PathManagerFixture, PathGenerated) {
+  // GIVEN a path generator with a costmap
+  auto const path_generator = pathing::Manager{std::move(mw_)};
+
+  // WHEN the generate path service is called with a reachable goal
+  auto path_request = std::make_shared<example_srvs::srv::GetPath::Request>();
+  path_request->start.data = {0, 0};
+  path_request->goal.data = {7, 7};
+  auto path_response = std::make_shared<example_srvs::srv::GetPath::Response>();
+  path_callback_(path_request, path_response);
+
+  // THEN the path generator should succeed
+  EXPECT_TRUE(path_response->success.data);
+  auto const expected = pathing::Path{{0, 0}, {1, 0}, {2, 0}, {3, 0}, {4, 0},
+                                      {5, 0}, {6, 0}, {7, 0}, {7, 1}, {7, 2},
+                                      {7, 3}, {7, 4}, {7, 5}, {7, 6}, {7, 7}};
+  // AND the path should be the same as the start
+  EXPECT_EQ(pathing::utilities::parseGeneratedPath(path_response->path),
+            expected);
+}
+
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();