-
Notifications
You must be signed in to change notification settings - Fork 18
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Port terrain planner benchmark to ROS 2
* Only ported the terrain_planner_benchmark_node so far, the rest are excluded Signed-off-by: Ryan Friedman <[email protected]>
- Loading branch information
Showing
11 changed files
with
171 additions
and
125 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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 | ||
| ``` |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,15 +1,15 @@ | ||
| <launch> | ||
| <arg name="location" default="dischma"/> | ||
| <arg name="location" default="davosdorf"/> | ||
| <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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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 | ||
| ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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); | ||
| } | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.