Create optimistic rollouts for CTPs

domain/BUILD

@@ -4,6 +4,7 @@ cc_library(
   name = "canadian_traveler",
   hdrs = ["canadian_traveler.hh"],
   srcs = ["canadian_traveler.cc"],
+  visibility = ["//visibility:public"],
   deps = ["@eigen"],
 )
 

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",
+  ]
+)

experimental/ctp/compute_optimistic_rollout.cc

@@ -0,0 +1,29 @@
+
+#include "experimental/ctp/compute_optimistic_rollout.hh"
+
+#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
+    }
+
+    return out;
+}
+}  // namespace robot::experimental::ctp

experimental/ctp/compute_optimistic_rollout.hh

@@ -0,0 +1,18 @@
+
+#pragma once
+
+#include "domain/canadian_traveler.hh"
+
+namespace robot::experimental::ctp {
+
+struct Rollout {
+    std::vector<int> path;
+    double cost;
+};
+
+Rollout compute_optimistic_rollout(const domain::CanadianTravelerGraph &graph,
+                                   const int initial_state, const int goal_state,
+                                   const domain::CanadianTravelerGraph::Weather &underlying_weather,
+                                   const domain::CanadianTravelerGraph::Weather &initial_belief);
+
+}  // namespace robot::experimental::ctp

experimental/ctp/compute_optimistic_rollout_test.cc

@@ -0,0 +1,78 @@
+
+#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<CTG> 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<CTG::Node> nodes = {{{0, 10}}, {{10, 10}}, {{10, 0}}, {{0, 0}}};
+
+    std::vector<CTG::Edge> 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}});
+    const auto uninformed_belief = graph_ptr->create_weather();
+
+    // Action
+    const auto rollout =
+        compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather, uninformed_belief);
+
+    // 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}});
+    const auto uninformed_belief = graph_ptr->create_weather();
+
+    // Action
+    const auto rollout =
+        compute_optimistic_rollout(*graph_ptr, START_IDX, GOAL_IDX, weather, uninformed_belief);
+
+    // 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