Update RRT::plan to use tl

Author: eholum

roboplan_rrt/include/roboplan_rrt/rrt.hpp

@@ -45,14 +45,12 @@ class RRT {
   /// @param options A struct containing RRT options.
   RRT(const std::shared_ptr<Scene> scene, const RRTOptions& options = RRTOptions());
 
-  tl::expected<JointPath, std::string> plan_expected(const JointConfiguration& start,
-                                                     const JointConfiguration& goal);
-
   /// @brief Plan a path from start to goal.
   /// @param start The starting joint configuration.
   /// @param goal The goal joint configuration.
-  /// @return A joint-space path, if planning succeeds, else std::nullopt.
-  std::optional<JointPath> plan(const JointConfiguration& start, const JointConfiguration& goal);
+  /// @return A joint-space path, if planning succeeds, otherwise an error message.
+  tl::expected<JointPath, std::string> plan(const JointConfiguration& start,
+                                            const JointConfiguration& goal);
 
   /// @brief Sets the seed for the random number generator (RNG).
   /// @details For reproducibility, this also seeds the underlying scene.

roboplan_rrt/src/rrt.cpp

@@ -1,4 +1,5 @@
 #include <chrono>
+#include <sstream>
 #include <stdexcept>
 
 #include <roboplan/core/path_utils.hpp>
@@ -32,28 +33,16 @@ RRT::RRT(const std::shared_ptr<Scene> scene, const RRTOptions& options)
   state_space_.set_bounds(lower_bounds, upper_bounds);
 };
 
-tl::expected<JointPath, std::string> RRT::plan_expected(const JointConfiguration& start,
-                                                        const JointConfiguration& goal) {
-  std::optional<JointPath> maybe_path = plan(start, goal);
-
-  if (maybe_path.has_value()) {
-    return maybe_path.value();
-  } else {
-    return tl::make_unexpected("Failed to find path!");
-  }
-}
-
-std::optional<JointPath> RRT::plan(const JointConfiguration& start,
-                                   const JointConfiguration& goal) {
+tl::expected<JointPath, std::string> RRT::plan(const JointConfiguration& start,
+                                               const JointConfiguration& goal) {
   std::cout << "Planning...\n";
 
   const auto& q_start = start.positions;
   const auto& q_goal = goal.positions;
 
   // Ensure the start and goal poses are valid
   if (!scene_->isValidPose(q_start) || !scene_->isValidPose(q_goal)) {
-    std::cout << "Invalid poses requested, cannot plan!\n";
-    return std::nullopt;
+    return tl::make_unexpected("Invalid poses requested, cannot plan!");
   }
 
   // Check whether direct connection between the start and goal are possible.
@@ -83,14 +72,18 @@ std::optional<JointPath> RRT::plan(const JointConfiguration& start,
     auto elapsed =
         std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
     if (options_.max_planning_time > 0 && options_.max_planning_time <= elapsed) {
-      std::cout << "RRT timed out after " << options_.max_planning_time << " seconds.\n";
-      break;
+      std::stringstream ss;
+      ss << "RRT timed out after " << options_.max_planning_time << " seconds.";
+      std::cout << ss.str() << "\n";
+      return tl::make_unexpected(ss.str());
     }
 
     // Check loop termination criteria.
     if (start_nodes_.size() + goal_nodes_.size() >= options_.max_nodes) {
-      std::cout << "Added maximum number of nodes (" << options_.max_nodes << ").\n";
-      break;
+      std::stringstream ss;
+      ss << "Added maximum number of nodes (" << options_.max_nodes << ").";
+      std::cout << ss.str() << "\n";
+      return tl::make_unexpected(ss.str());
     }
 
     // Set grow and target tree for this loop iteration.
@@ -126,8 +119,7 @@ std::optional<JointPath> RRT::plan(const JointConfiguration& start,
     }
   }
 
-  std::cout << "Unable to find a plan!\n";
-  return std::nullopt;
+  return tl::make_unexpected("Unable to find a path!");
 }
 
 void RRT::initializeTree(KdTree& tree, std::vector<Node>& nodes, const Eigen::VectorXd& q_init,