Skip to content

Commit

Permalink
Port terrain planner benchmark to ROS 2
Browse files Browse the repository at this point in the history
* Only ported the terrain_planner_benchmark_node so far, the rest are
  excluded

Signed-off-by: Ryan Friedman <[email protected]>
  • Loading branch information
Ryanf55 committed Nov 29, 2024
1 parent 21cee93 commit 6dd0c37
Show file tree
Hide file tree
Showing 11 changed files with 170 additions and 124 deletions.
123 changes: 50 additions & 73 deletions terrain_planner_benchmark/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
Empty file.
7 changes: 7 additions & 0 deletions terrain_planner_benchmark/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
# Terrain Planner Benchmark

## Terrain Planner Benchmark Node

```bash
ros2 launch terrain_planner_benchmark goaltype_benchmark.launch
```
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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");
Expand All @@ -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);
};

Expand Down
14 changes: 7 additions & 7 deletions terrain_planner_benchmark/launch/goaltype_benchmark.launch
Original file line number Diff line number Diff line change
Expand Up @@ -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>
12 changes: 7 additions & 5 deletions terrain_planner_benchmark/package.xml
Original file line number Diff line number Diff line change
@@ -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>
Expand All @@ -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>
Expand All @@ -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>
54 changes: 54 additions & 0 deletions terrain_planner_benchmark/src/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
52 changes: 29 additions & 23 deletions terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,45 +38,51 @@
* @author Jaeyoung Lim <[email protected]>
*/

#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <terrain_navigation/terrain_map.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/MarkerArray.h>

#include <geometry_msgs/msg/point.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include "terrain_planner/common.h"
#include "terrain_planner/terrain_ompl_rrt.h"
#include "terrain_planner/visualization.h"
#include "terrain_planner_benchmark/terrain_planner_benchmark.h"

int main(int argc, char** argv) {
ros::init(argc, argv, "ompl_rrt_planner");
ros::NodeHandle nh("");
ros::NodeHandle nh_private("~");
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("ompl_rrt_planner");

// Initialize ROS related publishers for visualization
auto start_pos_pub = nh.advertise<visualization_msgs::Marker>("start_position", 1, true);
auto goal_pos_pub = nh.advertise<visualization_msgs::Marker>("goal_position", 1, true);
auto path_pub = nh.advertise<nav_msgs::Path>("path", 1, true);
auto interpolate_path_pub = nh.advertise<nav_msgs::Path>("interpolated_path", 1, true);
auto path_segment_pub = nh.advertise<visualization_msgs::MarkerArray>("path_segments", 1, true);
auto grid_map_pub = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
auto trajectory_pub = nh.advertise<visualization_msgs::MarkerArray>("tree", 1, true);
std::string map_path, color_file_path, output_file_dir, location;
int number_of_runs;
nh_private.param<std::string>("map_path", map_path, "");
nh_private.param<std::string>("location", location, "");
nh_private.param<std::string>("color_file_path", color_file_path, "");
nh_private.param<std::string>("output_directory", output_file_dir, "");
nh_private.param<int>("number_of_runs", number_of_runs, 10);
auto start_pos_pub =
node->create_publisher<visualization_msgs::msg::Marker>("start_position", rclcpp::QoS(1).transient_local());
auto goal_pos_pub =
node->create_publisher<visualization_msgs::msg::Marker>("goal_position", rclcpp::QoS(1).transient_local());
auto path_pub = node->create_publisher<nav_msgs::msg::Path>("path", rclcpp::QoS(1).transient_local());
auto interpolate_path_pub =
node->create_publisher<nav_msgs::msg::Path>("interpolated_path", rclcpp::QoS(1).transient_local());
auto path_segment_pub =
node->create_publisher<visualization_msgs::msg::MarkerArray>("path_segments", rclcpp::QoS(1).transient_local());
auto grid_map_pub = node->create_publisher<grid_map_msgs::msg::GridMap>("grid_map", rclcpp::QoS(1).transient_local());
auto trajectory_pub =
node->create_publisher<visualization_msgs::msg::MarkerArray>("tree", rclcpp::QoS(1).transient_local());

auto const map_path = node->declare_parameter("map_path", "");
auto const location = node->declare_parameter("location", "");
auto const color_file_path = node->declare_parameter("color_file_path", "");
auto const output_file_dir = node->declare_parameter("output_directory", "");
auto const number_of_runs = node->declare_parameter("number_of_runs", 10);

// Load terrain map from defined tif paths
auto terrain_map = std::make_shared<TerrainMap>();
terrain_map->initializeFromGeotiff(map_path, false);
if (!terrain_map->initializeFromGeotiff(map_path)) {
RCLCPP_ERROR_STREAM(node->get_logger(), "Unable to load geotiff from '" << map_path << "'!");
return 1;
}
if (!color_file_path.empty()) { // Load color layer if the color path is nonempty
terrain_map->addColorFromGeotiff(color_file_path);
}
Expand Down
4 changes: 2 additions & 2 deletions terrain_planner_benchmark/src/test_ics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -164,8 +164,8 @@ int main(int argc, char** argv) {

auto grid_map_pub_ = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
auto reference_map_pub_ = nh.advertise<grid_map_msgs::GridMap>("reference_map", 1, true);
auto yaw_pub = nh.advertise<visualization_msgs::Marker>("yaw", 1, true);
auto circle_pub = nh.advertise<visualization_msgs::Marker>("circle", 1, true);
auto yaw_pub = nh.advertise<visualization_msgs::msg::Marker>("yaw", 1, true);
auto circle_pub = nh.advertise<visualization_msgs::msg::Marker>("circle", 1, true);

std::string map_path, map_color_path, output_file_dir, location;
bool visualize{true};
Expand Down
Loading

0 comments on commit 6dd0c37

Please sign in to comment.