Port terrain planner benchmark to ros2

terrain_navigation_ros/CMakeLists.txt

@@ -46,7 +46,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
-find_package(Boost REQUIRED COMPONENTS serialization system filesystem)
+find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 if(CMAKE_SYSTEM_NAME STREQUAL Linux)

terrain_planner/CMakeLists.txt

@@ -33,7 +33,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(grid_map_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
-find_package(Boost REQUIRED COMPONENTS serialization system filesystem)
+find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(ompl REQUIRED)
@@ -74,6 +74,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC
   grid_map_core
   grid_map_geo
   terrain_navigation
+  Boost
 )
 
 # Executables
@@ -176,6 +177,12 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+ament_export_dependencies(
+  Boost
+  tf2_geometry_msgs
+  tf2_eigen
+)
+
 # Test
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)

terrain_planner_benchmark/CMakeLists.txt

@@ -1,88 +1,65 @@
-cmake_minimum_required(VERSION 2.8.12)
+cmake_minimum_required(VERSION 3.14.4)
 project(terrain_planner_benchmark)
 
-add_definitions(-std=c++17 -Wall)
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-find_package(GDAL)
-find_package(OpenCV REQUIRED)
-find_package(ompl REQUIRED)
-
-find_package(catkin REQUIRED COMPONENTS
-    eigen_catkin
-    grid_map_core
-    grid_map_cv
-    grid_map_msgs
-    grid_map_ros
-    grid_map_pcl
-    grid_map_geo
-    terrain_navigation
-    terrain_planner
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES terrain_planner_benchmark
-)
-
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-  include
-  ${Boost_INCLUDE_DIR}
-  ${Eigen_INCLUDE_DIRS}
-  ${GeographicLib_INCLUDE_DIRS}
-  ${OMPL_INCLUDE_DIR}
-)
+# Set policy for 3.16 for CMP0076 defaulting to ON
+cmake_policy(VERSION 3.16)
 
-add_library(${PROJECT_NAME}
-  src/terrain_planner_benchmark.cpp
-)
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES})
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
-add_executable(terrain_planner_benchmark_node
-  src/terrain_planner_benchmark_node.cpp
-)
-add_dependencies(terrain_planner_benchmark_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(terrain_planner_benchmark_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} pthread)
+find_package(ament_cmake REQUIRED)
+find_package(GDAL 3.5)
+find_package(OpenCV REQUIRED)
+find_package(ompl REQUIRED)
 
-add_executable(ompl_benchmark_node
-  src/ompl_benchmark_node.cpp
-)
-add_dependencies(ompl_benchmark_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(ompl_benchmark_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} pthread)
+find_package(grid_map_core REQUIRED)
+find_package(grid_map_cv REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(grid_map_ros REQUIRED)
+find_package(grid_map_pcl REQUIRED)
+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)
 
-add_executable(test_ics
-  src/test_ics.cpp
-)
-add_dependencies(test_ics ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(test_ics ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
+add_library(${PROJECT_NAME})
+add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
 
-add_executable(test_rrt_replanning_node
-  src/test_rrt_replanning_node.cpp
+target_include_directories(${PROJECT_NAME}
+  PUBLIC 
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
 )
-add_dependencies(test_rrt_replanning_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(test_rrt_replanning_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
 
-add_executable(surface_visualization
-  src/surface_visualization.cpp
-)
-add_dependencies(surface_visualization ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(surface_visualization ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
+add_executable(terrain_planner_benchmark_node)
+add_executable(ompl_benchmark_node EXCLUDE_FROM_ALL)
+add_executable(test_ics EXCLUDE_FROM_ALL)
+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_subdirectory(src)
 
-add_executable(test_dubins_classification
-  src/test_dubins_classification.cpp
+include(GNUInstallDirs)
+install(
+  TARGETS
+    terrain_planner_benchmark_node
+  EXPORT export_${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
 )
-add_dependencies(test_dubins_classification ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(test_dubins_classification ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
 
-add_executable(run_replay
-  src/rviz_replay.cpp
+install(DIRECTORY
+  launch
+  resource
+  DESTINATION share/${PROJECT_NAME}/
 )
-add_dependencies(run_replay ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(run_replay ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
 
-add_executable(test_rrt_circle_goal
-  src/test_rrt_circle_goal.cpp
-)
-add_dependencies(test_rrt_circle_goal terrain_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
-target_link_libraries(test_rrt_circle_goal ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
+ament_package()

terrain_planner_benchmark/README.md

@@ -0,0 +1,7 @@
+# Terrain Planner Benchmark
+
+## Terrain Planner Benchmark Node
+
+```bash
+ros2 launch terrain_planner_benchmark goaltype_benchmark.launch
+```

terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h

@@ -55,8 +55,8 @@ class ReplayRunner {
         nh_.subscribe("/grid_map", 1, &ReplayRunner::GridmapCallback, this, ros::TransportHints().tcpNoDelay());
     vehicle_position_sub_ = nh_.subscribe("/planner_status", 10, &ReplayRunner::plannerStatusCallback, this);
     planner_status_pub_ = nh_.advertise<planner_msgs::NavigationStatus>("planner_status2", 1);
-    path_segment_pub_ = nh_.advertise<visualization_msgs::Marker>("visualized_path", 1);
-    reference_visual_pub_ = nh_.advertise<visualization_msgs::Marker>("visualized_reference", 1);
+    path_segment_pub_ = nh_.advertise<visualization_msgs::msg::Marker>("visualized_path", 1);
+    reference_visual_pub_ = nh_.advertise<visualization_msgs::msg::Marker>("visualized_reference", 1);
     terrain_info_pub_ = nh_.advertise<planner_msgs::TerrainInfo>("terrain_info", 1);
     double statusloop_dt_ = 0.05;
     ros::TimerOptions statuslooptimer_options(
@@ -133,10 +133,10 @@ class ReplayRunner {
     }
 
     // Visualize vehicle path and path reference
-    visualization_msgs::Marker marker = trajectory2MarkerMsg(vehicle_history_, 0, Eigen::Vector3d(1.0, 0.0, 1.0));
+    visualization_msgs::msg::Marker marker = trajectory2MarkerMsg(vehicle_history_, 0, Eigen::Vector3d(1.0, 0.0, 1.0));
     path_segment_pub_.publish(marker);
 
-    // visualization_msgs::Marker reference_marker = trajectory2MarkerMsg(reference_history_, 0, "viridis");
+    // visualization_msgs::msg::Marker reference_marker = trajectory2MarkerMsg(reference_history_, 0, "viridis");
     std::vector<Eigen::Vector3d> segment_colors;
     for (auto& segment_id : segment_history_) {
       const std::vector<std::vector<float>>& ctable = colorMap.at("gist_rainbow");
@@ -154,7 +154,7 @@ class ReplayRunner {
                                     static_cast<double>(rgb[2]));
       segment_colors.push_back(segment_color);
     }
-    visualization_msgs::Marker reference_marker = trajectory2MarkerMsg(reference_history_, 0, segment_colors);
+    visualization_msgs::msg::Marker reference_marker = trajectory2MarkerMsg(reference_history_, 0, segment_colors);
     reference_visual_pub_.publish(reference_marker);
   };
 

terrain_planner_benchmark/launch/goaltype_benchmark.launch

@@ -2,14 +2,14 @@
     <arg name="location" default="dischma"/>
     <arg name="runs" default="50"/>
 
-    <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
-    <node pkg="terrain_planner_benchmark" type="terrain_planner_benchmark_node" name="benchmark_planner" output="screen" required="true">
-        <param name="location" value="$(arg location)"/>
-        <param name="map_path" value="$(find terrain_models)/models/$(arg location).tif"/>
-        <param name="color_file_path" value="$(find terrain_models)/models/$(arg location)_color.tif"/>
-        <param name="output_directory" value="$(find terrain_planner)/../output"/>
-        <param name="number_of_runs" value="$(arg runs)"/>
+    <node pkg="terrain_planner_benchmark" exec="terrain_planner_benchmark_node" name="benchmark_planner" output="screen">
+        <param name="location" value="$(var location)"/>
+        <param name="map_path" value="$(find-pkg-share terrain_models)/models/$(var location).tif"/>
+        <param name="color_file_path" value="$(find-pkg-share terrain_models)/models/$(var location)_color.tif"/>
+        <param name="output_directory" value="$(find-pkg-share terrain_planner)/../output"/>
+        <param name="number_of_runs" value="$(var runs)"/>
         <param name="random" value="true"/>
     </node>
 </launch>

terrain_planner_benchmark/package.xml

@@ -1,4 +1,6 @@
-<package>
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>terrain_planner_benchmark</name>
   <version>0.0.1</version>
   <description>Benchmarking framework for terrain planners</description>
@@ -13,8 +15,7 @@
   <url type="bugtracker">https://github.com/Jaeyoung-Lim/steep-terrain-navigation/issues</url>
 
   <!-- Dependencies which this package needs to build itself. -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>eigen_catkin</build_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>grid_map_core</build_depend>
   <build_depend>grid_map_cv</build_depend>
   <build_depend>grid_map_ros</build_depend>
@@ -24,8 +25,9 @@
   <build_depend>terrain_navigation</build_depend>
   <build_depend>ompl</build_depend>
   <build_depend>terrain_planner</build_depend>
-  <run_depend>eigen_catkin</run_depend>
-  <run_depend>python3-gdal</run_depend>
+  <exec_depend>python3-gdal</exec_depend>
+
   <export>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

terrain_planner_benchmark/src/CMakeLists.txt

@@ -0,0 +1,54 @@
+target_sources(${PROJECT_NAME}
+  PRIVATE
+    terrain_planner_benchmark.cpp
+)
+target_link_libraries(${PROJECT_NAME}
+  PUBLIC
+    terrain_navigation::terrain_navigation
+    terrain_planner::terrain_planner
+    ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES}
+)
+
+target_sources(${PROJECT_NAME}_node
+  PRIVATE
+    terrain_planner_benchmark_node.cpp
+)
+target_link_libraries(${PROJECT_NAME}_node
+  PRIVATE
+    ${PROJECT_NAME}::${PROJECT_NAME} rclcpp::rclcpp ${geometry_msgs_TARGETS}
+)
+
+target_sources(ompl_benchmark_node
+  PRIVATE
+    ompl_benchmark_node.cpp
+)
+
+target_sources(test_ics
+  PRIVATE
+    test_ics.cpp
+)
+
+target_sources(test_rrt_replanning_node
+  PRIVATE
+    test_rrt_replanning_node.cpp
+)
+
+target_sources(surface_visualization
+  PRIVATE
+    surface_visualization.cpp
+)
+
+target_sources(test_dubins_classification
+  PRIVATE
+    test_dubins_classification.cpp
+)
+
+target_sources(run_replay
+  PRIVATE
+    rviz_replay.cpp
+)
+
+target_sources(test_rrt_circle_goal
+  PRIVATE
+    test_rrt_circle_goal.cpp
+)