Port example script for terrain_planner_benchmark

Fix launchfile

Fix CMake

F

Author: Jaeyoung-Lim

README.md

@@ -1,12 +1,14 @@
 <img align="right" height="60" src="https://user-images.githubusercontent.com/5248102/126074528-004a32b9-7911-486a-9e79-8b78e6e66fdc.png">
 
-# terrain-navigation
+# terrain-navigation2
 
 [![Build Test](https://github.com/ethz-asl/terrain-navigation/actions/workflows/build_test.yml/badge.svg)](https://github.com/ethz-asl/terrain-navigation/actions/workflows/build_test.yml)
 
 This package includes an implementation of the RA-L submission of  "Safe Low-Altitude Navigation in Steep Terrain with Fixed-Wing Aerial Vehicles".
 The implementation includes a global planner based on a RRT* in the Dubins Airplane space enabling low altitude navigation for fixed wing vehicles in steep terrain.
 
+This repository is a ROS 2 port of the original implementation in [terrain-navigation](https://github.com/ethz-asl/terrain-navigation)
+
 <p align="center">
     <img src="https://github.com/ethz-asl/terrain-navigation/assets/5248102/90e43b60-ea8c-49db-9fb3-257b145fc35c" alt="overview">
 </p>
@@ -57,6 +59,17 @@ Next, run the image, mounting in the source into a workspace. All the dependenci
 docker run --network=host -it -v $(pwd):/root/ros2_ws/src/ethz-asl/terrain-navigation -w /root/ros2_ws terrain-navigation-ros2 bash
 ```
 
+## Running an example of the planner
+In order to run the examples, build the `terrain_planner_benchmark` package.
+```
+colcon build --packages-up-to terrain_planner_benchmark
+```
+Run a simple planning example:
+```
+ros2 launch terrain_planner_benchmark test_ompl_rrt_circle_launch.xml 
+```
+
+
 ### Running the Build
 
 For dependencies that do not have binaries available, pull them into your ROS workspace using vcs.

terrain_planner/launch/test_ompl_dubins_to_circle.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <arg name="visualization" default="true"/>
+
+    <!-- <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/> -->
+    <node pkg="terrain_planner" exec="test_ompl_dubins_to_circle" name="test_ompl_dubins_to_circle">
+        <param name="map_path" value="$(find-pkg-share terrain_planner)/../resources/sertig.tif"/>
+        <param name="color_file_path" value="$(find-pkg-share terrain_planner)/../resources/sertig_color.tif"/>
+    </node>
+    <!-- <group if="$(arg visualization)">
+        <node type="rviz2" name="rviz2" pkg="rviz2" args="-d $(find terrain_planner)/launch/config_dubins_to_circle.rviz" />
+    </group> -->
+</launch>

terrain_planner/launch/test_ompl_dubins_to_circle_launch.xml

@@ -29,13 +29,19 @@ find_package(grid_map_geo REQUIRED)
 find_package(terrain_navigation REQUIRED)
 find_package(Boost REQUIRED) # Workaround for terrain_planner not exporting this correctly
 find_package(terrain_planner REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 add_library(${PROJECT_NAME})
 add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
 
 target_include_directories(${PROJECT_NAME}
   PUBLIC 
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  ${geometry_msgs_TARGETS}
 )
 
 add_executable(terrain_planner_benchmark_node)
@@ -45,13 +51,19 @@ add_executable(test_rrt_replanning_node EXCLUDE_FROM_ALL)
 add_executable(surface_visualization EXCLUDE_FROM_ALL)
 add_executable(test_dubins_classification EXCLUDE_FROM_ALL)
 add_executable(run_replay EXCLUDE_FROM_ALL)
-add_executable(test_rrt_circle_goal EXCLUDE_FROM_ALL)
+add_executable(test_rrt_circle_goal
+  src/test_rrt_circle_goal.cpp
+)
+target_link_libraries(test_rrt_circle_goal PUBLIC
+  ${PROJECT_NAME}
+)
 add_subdirectory(src)
 
 include(GNUInstallDirs)
 install(
   TARGETS
     terrain_planner_benchmark_node
+    test_rrt_circle_goal
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )

terrain_planner_benchmark/CMakeLists.txt

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/> -->
+  <node pkg="terrain_planner_benchmark" exec="test_rrt_circle_goal" name="rrt_planner" output="screen">
+    <param name="location" value="sertig"/>
+    <param name="map_path" value="$(find-pkg-share terrain_models)/models/sertig.tif"/>
+    <param name="color_file_path" value="$(find-pkg-share terrain_models)/models/sertig_color.tif"/>
+    <param name="output_directory" value="$(find-pkg-share terrain_planner_benchmark)/../output"/>
+  </node>
+  <!-- <node pkg="rviz2" exec="rviz2" name="rviz" args="-d $(find-pkg-share terrain_planner)/launch/config_ompl.rviz" /> -->
+</launch>

terrain_planner_benchmark/launch/test_ompl_rrt_circle.launch

@@ -25,6 +25,7 @@
   <build_depend>terrain_navigation</build_depend>
   <build_depend>ompl</build_depend>
   <build_depend>terrain_planner</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <exec_depend>python3-gdal</exec_depend>
 
   <export>

terrain_planner_benchmark/launch/test_ompl_rrt_circle_launch.xml

@@ -47,8 +47,3 @@ target_sources(run_replay
   PRIVATE
     rviz_replay.cpp
 )
-
-target_sources(test_rrt_circle_goal
-  PRIVATE
-    test_rrt_circle_goal.cpp
-)