From 520c22a42bbad11b79283da41d82e9599fccb080 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Wed, 25 Sep 2024 23:45:55 -0400 Subject: [PATCH 1/2] add initial unit tests --- domain/BUILD | 1 + experimental/ctp/BUILD | 20 +++++ .../ctp/compute_optimistic_rollout.cc | 13 ++++ .../ctp/compute_optimistic_rollout.hh | 17 +++++ .../ctp/compute_optimistic_rollout_test.cc | 74 +++++++++++++++++++ 5 files changed, 125 insertions(+) create mode 100644 experimental/ctp/BUILD create mode 100644 experimental/ctp/compute_optimistic_rollout.cc create mode 100644 experimental/ctp/compute_optimistic_rollout.hh create mode 100644 experimental/ctp/compute_optimistic_rollout_test.cc diff --git a/domain/BUILD b/domain/BUILD index 1e9e50b2..98c02569 100644 --- a/domain/BUILD +++ b/domain/BUILD @@ -4,6 +4,7 @@ cc_library( name = "canadian_traveler", hdrs = ["canadian_traveler.hh"], srcs = ["canadian_traveler.cc"], + visibility = ["//visibility:public"], deps = ["@eigen"], ) diff --git a/experimental/ctp/BUILD b/experimental/ctp/BUILD new file mode 100644 index 00000000..66de169f --- /dev/null +++ b/experimental/ctp/BUILD @@ -0,0 +1,20 @@ + +package(features=["warning_compile_flags"]) + +cc_library( + name = "compute_optimistic_rollout", + hdrs = ["compute_optimistic_rollout.hh"], + srcs = ["compute_optimistic_rollout.cc"], + deps = [ + "//domain:canadian_traveler", + ] +) + +cc_test( + name = "compute_optimistic_rollout_test", + srcs = ["compute_optimistic_rollout_test.cc"], + deps = [ + ":compute_optimistic_rollout", + "@com_google_googletest//:gtest_main", + ] +) diff --git a/experimental/ctp/compute_optimistic_rollout.cc b/experimental/ctp/compute_optimistic_rollout.cc new file mode 100644 index 00000000..b4b68fcd --- /dev/null +++ b/experimental/ctp/compute_optimistic_rollout.cc @@ -0,0 +1,13 @@ + +#include "experimental/ctp/compute_optimistic_rollout.hh" + +#include "domain/canadian_traveler.hh" + +namespace robot::experimental::ctp { + +Rollout compute_optimistic_rollout(const domain::CanadianTravelerGraph &graph, + const int initial_state, const int goal_state, + const domain::CanadianTravelerGraph::Weather &weather) { + return {}; +} +} // namespace robot::experimental::ctp diff --git a/experimental/ctp/compute_optimistic_rollout.hh b/experimental/ctp/compute_optimistic_rollout.hh new file mode 100644 index 00000000..0a77c1ea --- /dev/null +++ b/experimental/ctp/compute_optimistic_rollout.hh @@ -0,0 +1,17 @@ + +#pragma once + +#include "domain/canadian_traveler.hh" + +namespace robot::experimental::ctp { + +struct Rollout { + std::vector path; + double cost; +}; + +Rollout compute_optimistic_rollout(const domain::CanadianTravelerGraph &graph, + const int initial_state, const int goal_state, + const domain::CanadianTravelerGraph::Weather &weather); + +} // namespace robot::experimental::ctp diff --git a/experimental/ctp/compute_optimistic_rollout_test.cc b/experimental/ctp/compute_optimistic_rollout_test.cc new file mode 100644 index 00000000..28f0572d --- /dev/null +++ b/experimental/ctp/compute_optimistic_rollout_test.cc @@ -0,0 +1,74 @@ + +#include "experimental/ctp/compute_optimistic_rollout.hh" + +#include "domain/canadian_traveler.hh" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace robot::experimental::ctp { +namespace { +using CTG = domain::CanadianTravelerGraph; + +constexpr int START_IDX = 0; +constexpr int A_IDX = 1; +constexpr int B_IDX = 2; +constexpr int GOAL_IDX = 3; +std::shared_ptr create_test_graph() { + // Create the following graph. There exists a known traversable path from the start + // to the goal. There also exists a lower cost probabilistically traversable path + // that may be cheaper. + // + // S C=1 + // ├─────────┐A + // │ : + // C=10│ : C=1 + // │ : p=0.1 + // │ : + // │ : + // │ : + // ├─────────┘B + // G C=1 + // Start, A, B, Goal + std::vector nodes = {{{0, 10}}, {{10, 10}}, {{10, 0}}, {{0, 0}}}; + + std::vector edges = { + {.node_a = START_IDX, .node_b = GOAL_IDX, .cost = 10.0, .traversal_prob = {}}, + {.node_a = START_IDX, .node_b = A_IDX, .cost = 1.0, .traversal_prob = {}}, + {.node_a = A_IDX, .node_b = B_IDX, .cost = 1.0, .traversal_prob = 0.1}, + {.node_a = B_IDX, .node_b = GOAL_IDX, .cost = 1.0, .traversal_prob = {}}, + }; + + return CTG::create(std::move(nodes), std::move(edges)); +} +} // namespace + +TEST(ComputeOptimisticRolloutTest, rollout_in_traversable_weather) { + // Setup + const auto graph_ptr = create_test_graph(); + const auto weather = graph_ptr->create_weather( + {{.id_a = A_IDX, .id_b = B_IDX, .traversability = CTG::EdgeState::Traversable}}); + + // Action + const auto rollout = compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather); + + // Verification + constexpr double TOL = 1e-6; + EXPECT_THAT(rollout.path, testing::ElementsAre(START_IDX, A_IDX, B_IDX, GOAL_IDX)); + EXPECT_NEAR(rollout.cost, 3.0, TOL); +} + +TEST(ComputeOptimisticRolloutTest, rollout_in_untraversable_weather) { + // Setup + const auto graph_ptr = create_test_graph(); + const auto weather = graph_ptr->create_weather( + {{.id_a = A_IDX, .id_b = B_IDX, .traversability = CTG::EdgeState::Untraversable}}); + + // Action + const auto rollout = compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather); + + // Verification + constexpr double TOL = 1e-6; + EXPECT_THAT(rollout.path, testing::ElementsAre(START_IDX, A_IDX, START_IDX, GOAL_IDX)); + EXPECT_NEAR(rollout.cost, 12.0, TOL); +} +} // namespace robot::experimental::ctp From 6ff673ffc0f3ac560001b868bb1bc7dfed07ffd0 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Wed, 25 Sep 2024 23:57:24 -0400 Subject: [PATCH 2/2] start to implement rollout function --- .../ctp/compute_optimistic_rollout.cc | 24 +++++++++++++++---- .../ctp/compute_optimistic_rollout.hh | 3 ++- .../ctp/compute_optimistic_rollout_test.cc | 8 +++++-- 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/experimental/ctp/compute_optimistic_rollout.cc b/experimental/ctp/compute_optimistic_rollout.cc index b4b68fcd..6978a3f3 100644 --- a/experimental/ctp/compute_optimistic_rollout.cc +++ b/experimental/ctp/compute_optimistic_rollout.cc @@ -4,10 +4,26 @@ #include "domain/canadian_traveler.hh" namespace robot::experimental::ctp { +namespace { +using CTG = domain::CanadianTravelerGraph; +} + +Rollout compute_optimistic_rollout([[maybe_unused]] const CTG &graph, const int initial_state, + const int goal_state, + [[maybe_unused]] const CTG::Weather &underlying_weather, + [[maybe_unused]] const CTG::Weather &initial_belief) { + Rollout out = { + .path = {initial_state}, + .cost = 0.0, + }; + + while (out.path.back() != goal_state) { + // Observe at the current node given the underlying weather + // Create an optimistic belief + // Run Djikstra towards to goal assuming the belief + // Step in the best direction + } -Rollout compute_optimistic_rollout(const domain::CanadianTravelerGraph &graph, - const int initial_state, const int goal_state, - const domain::CanadianTravelerGraph::Weather &weather) { - return {}; + return out; } } // namespace robot::experimental::ctp diff --git a/experimental/ctp/compute_optimistic_rollout.hh b/experimental/ctp/compute_optimistic_rollout.hh index 0a77c1ea..0c970dd9 100644 --- a/experimental/ctp/compute_optimistic_rollout.hh +++ b/experimental/ctp/compute_optimistic_rollout.hh @@ -12,6 +12,7 @@ struct Rollout { Rollout compute_optimistic_rollout(const domain::CanadianTravelerGraph &graph, const int initial_state, const int goal_state, - const domain::CanadianTravelerGraph::Weather &weather); + const domain::CanadianTravelerGraph::Weather &underlying_weather, + const domain::CanadianTravelerGraph::Weather &initial_belief); } // namespace robot::experimental::ctp diff --git a/experimental/ctp/compute_optimistic_rollout_test.cc b/experimental/ctp/compute_optimistic_rollout_test.cc index 28f0572d..70265538 100644 --- a/experimental/ctp/compute_optimistic_rollout_test.cc +++ b/experimental/ctp/compute_optimistic_rollout_test.cc @@ -47,9 +47,11 @@ TEST(ComputeOptimisticRolloutTest, rollout_in_traversable_weather) { const auto graph_ptr = create_test_graph(); const auto weather = graph_ptr->create_weather( {{.id_a = A_IDX, .id_b = B_IDX, .traversability = CTG::EdgeState::Traversable}}); + const auto uninformed_belief = graph_ptr->create_weather(); // Action - const auto rollout = compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather); + const auto rollout = + compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather, uninformed_belief); // Verification constexpr double TOL = 1e-6; @@ -62,9 +64,11 @@ TEST(ComputeOptimisticRolloutTest, rollout_in_untraversable_weather) { const auto graph_ptr = create_test_graph(); const auto weather = graph_ptr->create_weather( {{.id_a = A_IDX, .id_b = B_IDX, .traversability = CTG::EdgeState::Untraversable}}); + const auto uninformed_belief = graph_ptr->create_weather(); // Action - const auto rollout = compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather); + const auto rollout = + compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather, uninformed_belief); // Verification constexpr double TOL = 1e-6;