From 6231ac7cd46388790f3ae82d7d8adb94f7463453 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Sun, 11 Aug 2024 21:24:45 +0200
Subject: [PATCH] Specify goal loiter direction

---
 .../terrain_planner/terrain_ompl_rrt.h        | 15 +++++++
 terrain_planner/src/terrain_ompl_rrt.cpp      | 39 +++++++++++++++++++
 2 files changed, 54 insertions(+)

diff --git a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h
index 19505a33..de4f533c 100644
--- a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h
+++ b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h
@@ -85,6 +85,21 @@ class TerrainOmplRrt {
    */
   void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& goal, double start_loiter_radius);
 
+  /**
+   * @brief Setup problem with center position of start and goal loiter circle with specific radius
+   *
+   * @param start_pos
+   * @param goal
+   * @param start_loiter_radius
+   *          - Positive: anti clockwise
+   *          - Negative: Clockwise
+   * @param goal_loiter_radius
+   *          - Positive: anti clockwise
+   *          - Negative: Clockwise
+   */
+  void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& goal, double start_loiter_radius,
+                    double goal_loiter_radius);
+
   /**
    * @brief Setup problem with position, velocity of the start and center of the goal loiter circle
    *
diff --git a/terrain_planner/src/terrain_ompl_rrt.cpp b/terrain_planner/src/terrain_ompl_rrt.cpp
index bca1bd3a..e4fb85a4 100644
--- a/terrain_planner/src/terrain_ompl_rrt.cpp
+++ b/terrain_planner/src/terrain_ompl_rrt.cpp
@@ -118,6 +118,45 @@ void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen:
   // std::cout << "Planner Range: " << planner_ptr->as<ompl::geometric::RRTstar>()->getRange() << std::endl;
 }
 
+void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& goal,
+                                  double start_loiter_radius, double goal_loiter_radius) {
+  configureProblem();
+  double radius =
+      problem_setup_->getStateSpace()->as<fw_planning::spaces::DubinsAirplaneStateSpace>()->getMinTurningRadius();
+  double delta_theta = 0.1;
+  for (double theta = -M_PI; theta < M_PI; theta += (delta_theta * 2 * M_PI)) {
+    ompl::base::ScopedState<fw_planning::spaces::DubinsAirplaneStateSpace> start_ompl(
+        problem_setup_->getSpaceInformation());
+
+    start_ompl->setX(start_pos(0) + std::abs(start_loiter_radius) * std::cos(theta));
+    start_ompl->setY(start_pos(1) + std::abs(start_loiter_radius) * std::sin(theta));
+    start_ompl->setZ(start_pos(2));
+    double start_yaw = bool(start_loiter_radius > 0) ? theta - M_PI_2 : theta + M_PI_2;
+    wrap_pi(start_yaw);
+    start_ompl->setYaw(start_yaw);
+    problem_setup_->addStartState(start_ompl);
+  }
+
+  goal_states_ = std::make_shared<ompl::base::GoalStates>(problem_setup_->getSpaceInformation());
+  for (double theta = -M_PI; theta < M_PI; theta += (delta_theta * 2 * M_PI)) {
+    ompl::base::ScopedState<fw_planning::spaces::DubinsAirplaneStateSpace> goal_ompl(
+        problem_setup_->getSpaceInformation());
+    goal_ompl->setX(goal(0) + std::abs(goal_loiter_radius) * std::cos(theta));
+    goal_ompl->setY(goal(1) + std::abs(goal_loiter_radius) * std::sin(theta));
+    goal_ompl->setZ(goal(2));
+    double goal_yaw = bool(goal_loiter_radius > 0) ? theta - M_PI_2 : theta + M_PI_2;
+    wrap_pi(goal_yaw);
+    goal_ompl->setYaw(goal_yaw);
+    goal_states_->addState(goal_ompl);
+  }
+  problem_setup_->setGoal(goal_states_);
+
+  problem_setup_->setup();
+
+  auto planner_ptr = problem_setup_->getPlanner();
+  // std::cout << "Planner Range: " << planner_ptr->as<ompl::geometric::RRTstar>()->getRange() << std::endl;
+}
+
 void TerrainOmplRrt::setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel,
                                   const Eigen::Vector3d& goal, double goal_radius) {
   configureProblem();