Specify goal loiter direction

ethz-asl · Aug 11, 2024 · 6231ac7 · 6231ac7
1 parent d4f68f4
commit 6231ac7
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 0 deletions.
diff --git 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
@@ -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();