From 0603afbd230dad7e675283090818b4424bae4291 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Wed, 6 Mar 2024 11:54:37 -0500 Subject: [PATCH] bind belief road map planner --- experimental/beacon_sim/BUILD | 45 +++++++++++++++++ .../belief_road_map_planner_python.cc | 36 ++++++++++++++ .../belief_road_map_planner_python_test.py | 49 +++++++++++++++++++ .../beacon_sim/robot_belief_python.cc | 14 ++++++ .../beacon_sim/test_helpers_python.cc | 15 ++++++ 5 files changed, 159 insertions(+) create mode 100644 experimental/beacon_sim/belief_road_map_planner_python.cc create mode 100644 experimental/beacon_sim/belief_road_map_planner_python_test.py create mode 100644 experimental/beacon_sim/robot_belief_python.cc create mode 100644 experimental/beacon_sim/test_helpers_python.cc diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index de0a70f8..d9df042e 100644 --- a/experimental/beacon_sim/BUILD +++ b/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"], diff --git a/experimental/beacon_sim/belief_road_map_planner_python.cc b/experimental/beacon_sim/belief_road_map_planner_python.cc new file mode 100644 index 00000000..87359432 --- /dev/null +++ b/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 +constexpr void bind_brm_plan(const auto &m, const std::string &type) { + const std::string type_name = "BeliefRoadMapPlan" + type; + py::class_>(m, type_name.c_str()) + .def_readwrite("nodes", &planning::BRMPlan::nodes) + .def_readwrite("beliefs", &planning::BRMPlan::beliefs); +} + +PYBIND11_MODULE(belief_road_map_planner_python, m) { + py::module_::import("experimental.beacon_sim.robot_belief_python"); + bind_brm_plan(m, "RobotBelief"); + py::class_(m, "BeliefRoadMapOptions") + .def(py::init<>()) + .def(py::init, int, + std::optional>(), + "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 diff --git a/experimental/beacon_sim/belief_road_map_planner_python_test.py b/experimental/beacon_sim/belief_road_map_planner_python_test.py new file mode 100644 index 00000000..8dd966d7 --- /dev/null +++ b/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() diff --git a/experimental/beacon_sim/robot_belief_python.cc b/experimental/beacon_sim/robot_belief_python.cc new file mode 100644 index 00000000..5dfa5b4a --- /dev/null +++ b/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_(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 diff --git a/experimental/beacon_sim/test_helpers_python.cc b/experimental/beacon_sim/test_helpers_python.cc new file mode 100644 index 00000000..c1913a14 --- /dev/null +++ b/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