From f66984f907b5dd2dc9fa14fc81cdea4f03eb2bd5 Mon Sep 17 00:00:00 2001
From: Jaeyoung Lim <jalim@ethz.ch>
Date: Fri, 14 Jun 2024 10:15:45 +0200
Subject: [PATCH] Fix planner example (#66)

* Speedup example

* Update readme
---
 README.md                                     | 14 +++++++++++
 terrain_planner/launch/config_ompl.rviz       | 25 +++++++++----------
 .../launch/test_ompl_rrt_circle.launch        |  2 +-
 .../src/test_rrt_circle_goal.cpp              |  4 +--
 4 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/README.md b/README.md
index 4414ae5b..e5006382 100644
--- a/README.md
+++ b/README.md
@@ -80,6 +80,20 @@ Build the package
 catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False
 catkin build -j$(nproc) -l$(nproc) terrain_navigation_ros
 ```
+## Running an example of the planner
+In order to run the examples, build the `terrain_planner_benchmark` package.
+```
+catkin build terrain_planner_benchmark
+```
+Run a simple planning example:
+```
+roslaunch terrain_planner_benchmark test_ompl_rrt_circle.launch 
+```
+
+<p align="center">
+    <img src="https://github.com/ethz-asl/terrain-navigation/assets/5248102/f630d1da-37c1-4e8d-9127-83a07ba22f45" alt="example" width="480">
+</p>
+
 
 ## Running with PX4 SITL(Software-In-The-Loop)
 
diff --git a/terrain_planner/launch/config_ompl.rviz b/terrain_planner/launch/config_ompl.rviz
index 563a02b9..6eebc7f8 100644
--- a/terrain_planner/launch/config_ompl.rviz
+++ b/terrain_planner/launch/config_ompl.rviz
@@ -14,7 +14,7 @@ Panels:
         - /GridMap3
         - /MarkerArray1
       Splitter Ratio: 0.5
-    Tree Height: 746
+    Tree Height: 839
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -30,7 +30,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: ""
@@ -217,7 +216,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2900.530517578125
+      Distance: 4114.6494140625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -225,25 +224,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -124.8638916015625
-        Y: 185.3620147705078
-        Z: 263.6095886230469
+        X: -713.6013793945312
+        Y: -437.5011901855469
+        Z: 1430.1207275390625
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.41979649662971497
+      Pitch: 0.6597965359687805
       Target Frame: <Fixed Frame>
-      Yaw: 5.846344947814941
+      Yaw: 0.6381592154502869
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1043
+  Height: 1136
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001a100000375fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c40000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000047c000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -252,6 +251,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 330
-  Y: 0
+  Width: 1848
+  X: 72
+  Y: 27
diff --git a/terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch b/terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch
index ce27a98d..29ba8401 100644
--- a/terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch
+++ b/terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch
@@ -1,7 +1,7 @@
 <launch>
     <arg name="visualization" default="true"/>
     <!-- <arg name="location" default="dischma_valley"/> -->
-    <arg name="location" default="gotthard"/>
+    <arg name="location" default="sertig"/>
 
     <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
 
diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp
index 90135a64..9e63873b 100644
--- a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp
+++ b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp
@@ -255,7 +255,7 @@ int main(int argc, char** argv) {
   } else {
     throw std::runtime_error("Specified start position is NOT valid");
   }
-  Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) - 0.4 * map_width_x, map_pos(1) + 0.4 * map_width_y, 0.0)};
+  Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) - 0.3 * map_width_x, map_pos(1) + 0.3 * map_width_y, 0.0)};
   Eigen::Vector3d updated_goal;
   if (validatePosition(terrain_map, goal, updated_goal)) {
     goal = updated_goal;
@@ -265,7 +265,7 @@ int main(int argc, char** argv) {
   }
 
   planner->setupProblem(start, goal);
-  if (planner->Solve(100.0, path)) {
+  if (planner->Solve(10.0, path)) {
     std::cout << "[TestRRTCircleGoal] Found Solution!" << std::endl;
   } else {
     std::cout << "[TestRRTCircleGoal] Unable to find solution" << std::endl;