Skip to content

Commit

Permalink
Migrate di tests (#37)
Browse files Browse the repository at this point in the history 
* Port tests from di example

* Add alias for SetMap and GetPath for test

* Fix make_occupancy_map to use 255
  • Loading branch information
@griswaldbrooks
griswaldbrooks authored Sep 19, 2023
1 parent d8fee9d commit a3f0af1
Show file tree
Hide file tree
Showing 3 changed files with 228 additions and 5 deletions.
8 changes: 8 additions & 0 deletions functional_programming_tests/include/pathing/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
19 changes: 19 additions & 0 deletions functional_programming_tests/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
206 changes: 201 additions & 5 deletions functional_programming_tests/test/with_functional_programming.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@
#include <gtest/gtest.h>
#include <std_msgs/msg/u_int8_multi_array.hpp>

using SetMap = example_srvs::srv::SetMap;
using GetPath = example_srvs::srv::GetPath;

/**
* @brief Gets the test costmap.
*
Expand Down Expand Up @@ -148,7 +151,7 @@ TEST(GenerateGlobalPath, PathGenerated) {

TEST(PathingUtilitiesParseSetMap, EmptyRequest) {
// GIVEN a set map request
auto const request = std::make_shared<example_srvs::srv::SetMap::Request>();
auto const request = std::make_shared<SetMap::Request>();

// WHEN a completely empty request is parsed
auto const map = pathing::utilities::parseSetMapRequest(request);
Expand All @@ -157,8 +160,8 @@ TEST(PathingUtilitiesParseSetMap, EmptyRequest) {
EXPECT_FALSE(map.has_value());
}

std::shared_ptr<example_srvs::srv::SetMap::Request> createSetMapRequest() {
auto const request = std::make_shared<example_srvs::srv::SetMap::Request>();
std::shared_ptr<SetMap::Request> createSetMapRequest() {
auto const request = std::make_shared<SetMap::Request>();

request->map = std_msgs::msg::UInt8MultiArray();

Expand Down Expand Up @@ -254,7 +257,7 @@ TEST(GeneratePath, EmptyOccupancyMap) {
// GIVEN a GetPath request and an empty costmap
pathing::Map<unsigned char> const sample_occupancy_map;

auto const request = std::make_shared<example_srvs::srv::GetPath::Request>();
auto const request = std::make_shared<GetPath::Request>();

request->start.data = {0, 0};
request->goal.data = {1, 1};
Expand All @@ -272,7 +275,7 @@ TEST(GeneratePath, InvalidStartSize) {
// GIVEN a GetPath request with an incorrect start field and an occupancy map
auto const sample_occupancy_map = get_test_occupancy_map();

auto const request = std::make_shared<example_srvs::srv::GetPath::Request>();
auto const request = std::make_shared<GetPath::Request>();

request->start.data = {0, 0, 0};
request->goal.data = {1, 1};
Expand Down Expand Up @@ -336,6 +339,199 @@ 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<SetMap::Request> make_occupancy_map() {
auto const request = std::make_shared<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, 255, 0, 0, 0, 0, //
0, 0, 0, 255, 0, 0, 0, 0, //
0, 0, 255, 255, 255, 0, 0, 0, //
0, 0, 255, 0, 255, 255, 0, 0, //
0, 0, 255, 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<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<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<GetPath::Request>();
auto path_response = std::make_shared<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<GetPath::Request>();
path_request->start.data = {0, 0};
path_request->goal.data = {0, 0};
auto path_response = std::make_shared<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<GetPath::Request>();
path_request->start.data = {2, 2};
path_request->goal.data = {5, 5};

auto path_response = std::make_shared<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<GetPath::Request>();
path_request->start.data = {0, 0};
path_request->goal.data = {7, 7};
auto path_response = std::make_shared<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();
Expand Down

0 comments on commit a3f0af1

Please sign in to comment.