bind belief road map planner

experimental/beacon_sim/BUILD

@@ -410,6 +410,31 @@ cc_test(
   ],
 )
 
+pybind_extension(
+  name = "belief_road_map_planner_python",
+  srcs = ["belief_road_map_planner_python.cc"],
+  data = [
+    ":robot_belief_python.so"
+  ],
+  deps = [
+    ":belief_road_map_planner",
+  ]
+)
+
+py_test(
+  name = "belief_road_map_planner_python_test",
+  srcs = ["belief_road_map_planner_python_test.py"],
+  data = [
+    "//planning:probabilistic_road_map_python.so",
+    ":belief_road_map_planner_python.so",
+    ":ekf_slam_python.so",
+    ":test_helpers_python.so",
+  ],
+  deps = [
+  ]
+
+)
+
 cc_binary(
   name = "run_trials",
   srcs = ["run_trials.cc"],
@@ -725,6 +750,14 @@ cc_library(
   ]
 )
 
+pybind_extension(
+  name = "robot_belief_python",
+  srcs = ["robot_belief_python.cc"],
+  deps = [
+    ":robot_belief",
+  ]
+)
+
 cc_library(
     name = "test_helpers",
     hdrs = ["test_helpers.hh"],
@@ -737,6 +770,18 @@ cc_library(
     ]
 )
 
+pybind_extension(
+  name = "test_helpers_python",
+  srcs = ["test_helpers_python.cc"],
+  data = [
+    ":correlated_beacons_python.so",
+    "//planning:probabilistic_road_map_python.so",
+  ],
+  deps = [
+    ":test_helpers",
+  ]
+)
+
 cc_library(
     name = "information_lower_bound_search",
     hdrs = ["information_lower_bound_search.hh"],

experimental/beacon_sim/belief_road_map_planner_python.cc

@@ -0,0 +1,36 @@
+
+#include "common/time/robot_time.hh"
+#include "experimental/beacon_sim/belief_road_map_planner.hh"
+#include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace robot::experimental::beacon_sim {
+
+template <typename T>
+constexpr void bind_brm_plan(const auto &m, const std::string &type) {
+    const std::string type_name = "BeliefRoadMapPlan" + type;
+    py::class_<planning::BRMPlan<T>>(m, type_name.c_str())
+        .def_readwrite("nodes", &planning::BRMPlan<T>::nodes)
+        .def_readwrite("beliefs", &planning::BRMPlan<T>::beliefs);
+}
+
+PYBIND11_MODULE(belief_road_map_planner_python, m) {
+    py::module_::import("experimental.beacon_sim.robot_belief_python");
+    bind_brm_plan<RobotBelief>(m, "RobotBelief");
+    py::class_<BeliefRoadMapOptions>(m, "BeliefRoadMapOptions")
+        .def(py::init<>())
+        .def(py::init<double, std::optional<double>, int,
+                      std::optional<time::RobotTimestamp::duration>>(),
+             "max_sensor_range_m"_a, "uncertainty_tolerance"_a, "max_num_edge_transforms"_a,
+             "timeout"_a)
+        .def_readwrite("max_sensor_range_m", &BeliefRoadMapOptions::max_sensor_range_m)
+        .def_readwrite("uncertainty_tolerance", &BeliefRoadMapOptions::uncertainty_tolerance)
+        .def_readwrite("max_num_edge_transforms", &BeliefRoadMapOptions::max_num_edge_transforms)
+        .def_readwrite("timeout", &BeliefRoadMapOptions::timeout);
+
+    m.def("compute_belief_road_map_plan", &compute_belief_road_map_plan);
+}
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/belief_road_map_planner_python_test.py

@@ -0,0 +1,49 @@
+
+import unittest
+
+import experimental.beacon_sim.belief_road_map_planner_python as brm
+import experimental.beacon_sim.test_helpers_python as helpers
+import experimental.beacon_sim.ekf_slam_python as esp
+
+class BeliefRoadMapPlannerPythonTest(unittest.TestCase):
+    def test_belief_road_map_planner(self):
+        # Setup
+        ekf_config = esp.EkfSlamConfig(
+            max_num_beacons=11,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.05,
+            cross_track_process_noise_m_per_rt_meter=0.05,
+            pos_process_noise_m_per_rt_s=0.0,
+            heading_process_noise_rad_per_rt_meter=1e-3,
+            heading_process_noise_rad_per_rt_s=0.0,
+            beacon_pos_process_noise_m_per_rt_s=1e-6,
+            range_measurement_noise_m=1e-1,
+            bearing_measurement_noise_rad=1e-1,
+            on_map_load_position_uncertainty_m=2.0,
+            on_map_load_heading_uncertainty_rad=0.5,
+        )
+
+        brm_options = brm.BeliefRoadMapOptions(
+            max_sensor_range_m=3.0,
+            uncertainty_tolerance=None,
+            max_num_edge_transforms=1000,
+            timeout=None)
+
+        P_LONE_BEACON = 0.5
+        P_STACKED_BEACON = 0.1
+        P_NO_STACK_BEACON = 0.01
+        road_map, ekf, potential = helpers.create_diamond_environment(
+            ekf_config, P_LONE_BEACON, P_NO_STACK_BEACON, P_STACKED_BEACON
+        )
+
+        # Action
+        plan = brm.compute_belief_road_map_plan(road_map, ekf, potential, brm_options)
+
+        # Verification
+        self.assertIsNotNone(plan)
+        self.assertNotIn(1, plan.nodes)
+
+
+
+if __name__ == "__main__":
+    unittest.main()

experimental/beacon_sim/robot_belief_python.cc

@@ -0,0 +1,14 @@
+
+#include "experimental/beacon_sim/robot_belief.hh"
+#include "pybind11/eigen.h"
+#include "pybind11/pybind11.h"
+
+namespace py = pybind11;
+
+namespace robot::experimental::beacon_sim {
+PYBIND11_MODULE(robot_belief_python, m) {
+    py::class_<RobotBelief>(m, "RobotBelief")
+        .def_readwrite("local_from_robot", &RobotBelief::local_from_robot)
+        .def_readwrite("cov_in_robot", &RobotBelief::cov_in_robot);
+}
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/test_helpers_python.cc

@@ -0,0 +1,15 @@
+
+#include "experimental/beacon_sim/test_helpers.hh"
+#include "pybind11/pybind11.h"
+
+namespace py = pybind11;
+
+namespace robot::experimental::beacon_sim {
+PYBIND11_MODULE(test_helpers_python, m) {
+    py::module_::import("experimental.beacon_sim.correlated_beacons_python");
+    py::module_::import("planning.probabilistic_road_map_python");
+    m.def("create_grid_environment", &create_grid_environment);
+    m.def("create_diamond_environment", &create_diamond_environment);
+    m.def("create_stress_test_environment", &create_stress_test_environment);
+}
+}  // namespace robot::experimental::beacon_sim