Fix default initializations

Author: Jaeyoung-Lim

README.md

@@ -94,7 +94,7 @@ rosdep install --from-paths src --ignore-src
 
 Build the package
 ```bash
-colcon build --packages-up-to terrain_navigation_ros
+colcon build --packages-up-to terrain_navigation_ros --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
 ```
 
 ## Running with PX4 SITL(Software-In-The-Loop), Gazebo and QGroundControl (QGC)

terrain_planner/include/terrain_planner/terrain_ompl_rrt.h

@@ -50,7 +50,7 @@
 
 class TerrainOmplRrt {
  public:
-  TerrainOmplRrt();
+  TerrainOmplRrt(const double minTurnRadius = 66.7, const double maxPitchAngle = 0.3 * M_PI);
   TerrainOmplRrt(const ompl::base::StateSpacePtr& space);
   virtual ~TerrainOmplRrt();
 
@@ -70,7 +70,8 @@ class TerrainOmplRrt {
    *          - Negative: Clockwise
    */
   void setupProblem(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& goal) {
-    this->setupProblem(start_pos, goal, problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->getMinTurnRadius());
+    this->setupProblem(start_pos, goal,
+                       problem_setup_->getStateSpace()->as<ompl::base::OwenStateSpace>()->getMinTurnRadius());
   };
 
   /**

terrain_planner/src/terrain_ompl_rrt.cpp

@@ -35,9 +35,9 @@
 #include "terrain_planner/terrain_ompl_rrt.h"
 
 // Constructor
-TerrainOmplRrt::TerrainOmplRrt() {
-  problem_setup_ =
-      std::make_shared<ompl::OmplSetup>(ompl::base::StateSpacePtr(new ompl::base::OwenStateSpace(66.7, 0.3 * M_PI)));
+TerrainOmplRrt::TerrainOmplRrt(const double minTurnRadius, const double maxPitchAngle) {
+  problem_setup_ = std::make_shared<ompl::OmplSetup>(
+      ompl::base::StateSpacePtr(new ompl::base::OwenStateSpace(minTurnRadius, maxPitchAngle)));
 }
 TerrainOmplRrt::TerrainOmplRrt(const ompl::base::StateSpacePtr& space) {
   problem_setup_ = std::make_shared<ompl::OmplSetup>(space);