Hook ARA* to examples with 2d-grid examples

examples/path_planning/py_path_planning.cc

@@ -15,10 +15,12 @@
 #include <iostream>
 
 #include "pybind11/eigen.h"
+#include "pybind11/functional.h"
 #include "pybind11/pybind11.h"
 #include "pybind11/stl.h"
 
 #include "optimus/path_planning/action_set_2d.h"
+#include "optimus/path_planning/arastar_grid_2d_planner.h"
 #include "optimus/path_planning/astar_grid_2d_planner.h"
 #include "optimus/path_planning/astar_se2_planner.h"
 #include "optimus/path_planning/dstar_lite_grid_2d_planner.h"
@@ -53,7 +55,7 @@ class PyPlanner {
   }
 
  protected:
-  py::array_t<Grid2DScalar> grid_2d_;
+  PyGrid2D grid_2d_;
   std::unique_ptr<Grid2DMap> grid_2d_map_;
 };
 
@@ -139,23 +141,31 @@ using ExampleDStarLiteSE2Planner = ExampleSE2Planner<DStarLiteSE2Planner>;
 template <class Planner>
 class PyGrid2DPlanner : public PyPlanner<PyGrid2DPlanner<Planner>> {
  public:
-  explicit PyGrid2DPlanner(const Grid2DEnvironment::Config& config)
-      : planner_(config) {}
-
-  std::vector<Position2D> PlanPath(const Position2D& start,
-                                   const Position2D& goal) {
+  template <typename... Args>
+  explicit PyGrid2DPlanner(const Grid2DEnvironment::Config& config,
+                           Args&&... args)
+      : planner_(config, std::forward<Args>(args)...) {}
+
+  std::vector<Position2D> PlanPath(
+      const Position2D& start, const Position2D& goal,
+      const std::optional<UserCallback>& user_callback) {
     std::vector<Position2D> path;
     planning_time_ = 0;
     num_expansions_ = 0;
-    auto callback = [this](UserCallbackEvent event) {
+
+    auto callback = [this, &user_callback](UserCallbackEvent event) {
       if (PyErr_CheckSignals() != 0) {
         throw py::error_already_set();
       }
       if (event == UserCallbackEvent::kSearch) {
         ++num_expansions_;
       }
+      if (user_callback != std::nullopt) {
+        return user_callback.value()(event);
+      }
       return true;
     };
+
     const auto start_timestamp = std::chrono::steady_clock::now();
     if (auto status = planner_.PlanPath(start, goal, callback, path);
         status != PlannerStatus::kSuccess) {
@@ -169,15 +179,18 @@ class PyGrid2DPlanner : public PyPlanner<PyGrid2DPlanner<Planner>> {
   }
 
   std::vector<Position2D> ReplanPath(
-      const Position2D& start,
-      const std::vector<Position2D>& changed_positions) {
+      const Position2D& start, const std::vector<Position2D>& changed_positions,
+      const std::optional<UserCallback>& user_callback) {
     std::vector<Position2D> path;
     planning_time_ = 0;
     num_expansions_ = 0;
-    auto callback = [this](UserCallbackEvent event) {
+    auto callback = [this, &user_callback](UserCallbackEvent event) {
       if (event == UserCallbackEvent::kSearch) {
         ++num_expansions_;
       }
+      if (user_callback != std::nullopt) {
+        return user_callback.value()(event);
+      }
       return true;
     };
     const auto start_timestamp = std::chrono::steady_clock::now();
@@ -196,6 +209,7 @@ class PyGrid2DPlanner : public PyPlanner<PyGrid2DPlanner<Planner>> {
   auto planning_time() const { return planning_time_; }
   auto num_expansions() const { return num_expansions_; }
   auto path_cost() const { return planner_.GetPathCost(); }
+  auto epsilon() const { return planner_.epsilon(); }
 
   auto* mutable_planner() { return &planner_; }
 
@@ -207,6 +221,7 @@ class PyGrid2DPlanner : public PyPlanner<PyGrid2DPlanner<Planner>> {
 
 using PyAStarGrid2DPlanner = PyGrid2DPlanner<AStarGrid2DPlanner>;
 using PyDStarLiteGrid2DPlanner = PyGrid2DPlanner<DStarLiteGrid2DPlanner>;
+using PyAraStarGrid2DPlanner = PyGrid2DPlanner<AraStarGrid2DPlanner>;
 
 PYBIND11_MODULE(py_path_planning, m) {
   auto se2_environment = py::class_<SE2Environment>(m, "SE2Environment");
@@ -262,8 +277,11 @@ PYBIND11_MODULE(py_path_planning, m) {
 
   py::class_<PyAStarGrid2DPlanner>(m, "AStarGrid2DPlanner")
       .def(py::init<const Grid2DEnvironment::Config&>(), py::arg("config"))
-      .def("plan_path", &PyAStarGrid2DPlanner::PlanPath)
-      .def("replan_path", &PyAStarGrid2DPlanner::ReplanPath)
+      .def("plan_path", &PyAStarGrid2DPlanner::PlanPath, py::arg("start"),
+           py::arg("goal"), py::arg("user_callback") = std::nullopt)
+      .def("replan_path", &PyAStarGrid2DPlanner::ReplanPath, py::arg("start"),
+           py::arg("changed_positions"),
+           py::arg("user_callback") = std::nullopt)
       .def("set_grid_2d", &PyAStarGrid2DPlanner::SetGrid2D)
       .def_property_readonly("planning_time",
                              &PyAStarGrid2DPlanner::planning_time)
@@ -273,14 +291,47 @@ PYBIND11_MODULE(py_path_planning, m) {
 
   py::class_<PyDStarLiteGrid2DPlanner>(m, "DStarLiteGrid2DPlanner")
       .def(py::init<const Grid2DEnvironment::Config&>(), py::arg("config"))
-      .def("plan_path", &PyDStarLiteGrid2DPlanner::PlanPath)
-      .def("replan_path", &PyDStarLiteGrid2DPlanner::ReplanPath)
+      .def("plan_path", &PyDStarLiteGrid2DPlanner::PlanPath, py::arg("start"),
+           py::arg("goal"), py::arg("user_callback") = std::nullopt)
+      .def("replan_path", &PyDStarLiteGrid2DPlanner::ReplanPath,
+           py::arg("start"), py::arg("changed_positions"),
+           py::arg("user_callback") = std::nullopt)
       .def("set_grid_2d", &PyDStarLiteGrid2DPlanner::SetGrid2D)
       .def_property_readonly("planning_time",
                              &PyDStarLiteGrid2DPlanner::planning_time)
       .def_property_readonly("num_expansions",
                              &PyDStarLiteGrid2DPlanner::num_expansions)
       .def_property_readonly("path_cost", &PyDStarLiteGrid2DPlanner::path_cost);
+
+  py::class_<AraStarPlannerConfig>(m, "AraStarPlannerConfig")
+      .def(py::init<>())
+      .def_readwrite("epsilon_start", &AraStarPlannerConfig::epsilon_start)
+      .def_readwrite("epsilon_decrease_rate",
+                     &AraStarPlannerConfig::epsilon_decrease_rate)
+      .def("validate", &AraStarPlannerConfig::Validate);
+
+  py::class_<PyAraStarGrid2DPlanner>(m, "AraStarGrid2DPlanner")
+      .def(py::init<const Grid2DEnvironment::Config&,
+                    const AraStarPlannerConfig&>(),
+           py::arg("env_config"), py::arg("planner_config"))
+      .def("plan_path", &PyAraStarGrid2DPlanner::PlanPath, py::arg("start"),
+           py::arg("goal"), py::arg("user_callback") = std::nullopt)
+      .def("replan_path", &PyAraStarGrid2DPlanner::ReplanPath, py::arg("start"),
+           py::arg("changed_positions"),
+           py::arg("user_callback") = std::nullopt)
+      .def("set_grid_2d", &PyAraStarGrid2DPlanner::SetGrid2D)
+      .def_property_readonly("planning_time",
+                             &PyAraStarGrid2DPlanner::planning_time)
+      .def_property_readonly("num_expansions",
+                             &PyAraStarGrid2DPlanner::num_expansions)
+      .def_property_readonly("path_cost", &PyAraStarGrid2DPlanner::path_cost)
+      .def_property_readonly("epsilon", &PyAraStarGrid2DPlanner::epsilon);
+
+  py::enum_<UserCallbackEvent>(m, "UserCallbackEvent")
+      .value("SEARCH", UserCallbackEvent::kSearch)
+      .value("RECONSTRUCTION", UserCallbackEvent::kReconstruction)
+      .value("SOLUTION_FOUND", UserCallbackEvent::kSolutionFound)
+      .export_values();
 }
 
 }  // namespace optimus

examples/path_planning/run_grid_2d_planners.py

@@ -48,11 +48,11 @@ def draw_circle(p: numpy.ndarray, color: str, name: str):
   fig.show()
 
 
-def plan_path(planner, obstacle_data, start, goal):
+def plan_path(planner, obstacle_data, start, goal, callback=None):
   path = []
   try:
     planner.set_grid_2d(obstacle_data)
-    path = planner.plan_path(start, goal)
+    path = planner.plan_path(start, goal, callback)
     print(f'Planning time: {planner.planning_time:.3f} seconds')
     print(f'Number of expansions: {planner.num_expansions}')
     print(f'Path cost: {planner.path_cost:.3f}')
@@ -62,10 +62,10 @@ def plan_path(planner, obstacle_data, start, goal):
   return path
 
 
-def replan_path(planner, start, changed_positions):
+def replan_path(planner, start, changed_positions, callback=None):
   path = []
   try:
-    path = planner.replan_path(start, changed_positions)
+    path = planner.replan_path(start, changed_positions, callback)
     print(f'Replanning time: {planner.planning_time:.3f} seconds')
     print(f'Number of expansions: {planner.num_expansions}')
     print(f'Path cost: {planner.path_cost:.3f}')
@@ -75,6 +75,30 @@ def replan_path(planner, start, changed_positions):
   return path
 
 
+def compute_all_arastar_planner_improvements(planner, obstacle_data, start,
+                                             goal):
+
+  def callback(event):
+    return event != py_path_planning.UserCallbackEvent.SOLUTION_FOUND
+
+  names_to_paths = dict()
+
+  def append_path(path):
+    names_to_paths[f'epsilon={planner.epsilon}'] = path
+
+  # Timings are likely to be quote off due to signifant callback oveheads.
+  # The main point here is to illustrate path improvements.
+  path = plan_path(planner, obstacle_data, start, goal, callback)
+  append_path(path)
+  while True:
+    path = replan_path(planner, start, [], callback)
+    append_path(path)
+    if numpy.isclose(planner.epsilon, 1.):
+      break
+
+  return names_to_paths
+
+
 def main():
   obstacle_data = utils.load_obstacle_data()
   original_obstacle_data = numpy.copy(obstacle_data)
@@ -94,6 +118,13 @@ def main():
   print('Running D*Lite')
   dstar_lite_path = plan_path(dstar_lite_planner, obstacle_data, start, goal)
 
+  arastar_config = py_path_planning.AraStarPlannerConfig()
+  assert arastar_config.validate()
+  arastar_planner = py_path_planning.AraStarGrid2DPlanner(
+      env_config, arastar_config)
+  print('Running ARA*')
+  arastar_path = plan_path(arastar_planner, obstacle_data, start, goal)
+
   print('\n\nReplanning')
   # Simulate a moving robot: move a robot to a point approx. on the previously
   # found path.
@@ -113,15 +144,23 @@ def main():
   new_dstar_lite_path = replan_path(dstar_lite_planner, new_start,
                                     changed_positions)
 
-  plot_results({
-      'A*': astar_path,
-      'D*Lite': dstar_lite_path
-  }, original_obstacle_data, 'Planning')
+  plot_results(
+      {
+          'A*': astar_path,
+          'D*Lite': dstar_lite_path,
+          'ARA*': arastar_path
+      }, original_obstacle_data, 'Planning')
   plot_results({
       'A*': new_astar_path,
       'D*Lite': new_dstar_lite_path
   }, obstacle_data, 'Replanning')
 
+  print('\n\nARA* improvements')
+  arastar_improvement_paths = compute_all_arastar_planner_improvements(
+      arastar_planner, original_obstacle_data, start, goal)
+  plot_results(arastar_improvement_paths, original_obstacle_data,
+               'ARA* improvements')
+
 
 if __name__ == '__main__':
   main()

optimus/path_planning/arastar_grid_2d_planner.h

@@ -21,7 +21,8 @@
 
 namespace optimus {
 
-using AraStarGrid2DPlanner = Grid2DPlanner<AraStarPlanner<Grid2DEnvironment>>;
+using AraStarGrid2DPlanner =
+    Grid2DPlanner<AraStarPlanner<Grid2DEnvironment>, /*Anytime=*/true>;
 
 template <>
 std::optional<float> AraStarGrid2DPlanner::GetPathCost() const {

optimus/path_planning/grid_2d_planner.h

@@ -44,6 +44,8 @@ class Grid2DPlannerBase {
 
   virtual std::optional<float> GetPathCost() const = 0;
 
+  virtual float epsilon() const = 0;
+
  protected:
   int GetStateIndex(const Position2D& t) const;
   void ReconstructPath(const std::vector<int>& path_indices,
@@ -54,7 +56,7 @@ class Grid2DPlannerBase {
   std::optional<int> goal_index_;
 };
 
-template <class Algorithm>
+template <class Algorithm, bool Anytime = false>
 class Grid2DPlanner final : public Grid2DPlannerBase {
  public:
   template <typename... Args>
@@ -74,12 +76,20 @@ class Grid2DPlanner final : public Grid2DPlannerBase {
 
   std::optional<float> GetPathCost() const final;
 
+  float epsilon() const final {
+    if constexpr (Anytime) {
+      return algorithm_.epsilon();
+    } else {
+      return 1.;
+    }
+  }
+
  private:
   Algorithm algorithm_;
 };
 
-template <class Algorithm>
-PlannerStatus Grid2DPlanner<Algorithm>::PlanPath(
+template <class Algorithm, bool Anytime>
+PlannerStatus Grid2DPlanner<Algorithm, Anytime>::PlanPath(
     const Position2D& start, const Position2D& goal,
     const UserCallback& user_callback, std::vector<Position2D>& path) {
   start_index_.reset();