diff --git a/terrain_navigation_ros/CMakeLists.txt b/terrain_navigation_ros/CMakeLists.txt index 3daf7bbd..b8eb0c09 100644 --- a/terrain_navigation_ros/CMakeLists.txt +++ b/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) diff --git a/terrain_planner/CMakeLists.txt b/terrain_planner/CMakeLists.txt index 4019c4ca..4ec06506 100644 --- a/terrain_planner/CMakeLists.txt +++ b/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) diff --git a/terrain_planner_benchmark/CMakeLists.txt b/terrain_planner_benchmark/CMakeLists.txt index 7be76adc..5a53c4a0 100644 --- a/terrain_planner_benchmark/CMakeLists.txt +++ b/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 + "$" ) -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() diff --git a/terrain_planner_benchmark/COLCON_IGNORE b/terrain_planner_benchmark/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/terrain_planner_benchmark/README.md b/terrain_planner_benchmark/README.md new file mode 100644 index 00000000..2e599925 --- /dev/null +++ b/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 +``` diff --git a/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h b/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h index eb9ced32..afd5334b 100644 --- a/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h +++ b/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_status2", 1); - path_segment_pub_ = nh_.advertise("visualized_path", 1); - reference_visual_pub_ = nh_.advertise("visualized_reference", 1); + path_segment_pub_ = nh_.advertise("visualized_path", 1); + reference_visual_pub_ = nh_.advertise("visualized_reference", 1); terrain_info_pub_ = nh_.advertise("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 segment_colors; for (auto& segment_id : segment_history_) { const std::vector>& ctable = colorMap.at("gist_rainbow"); @@ -154,7 +154,7 @@ class ReplayRunner { static_cast(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); }; diff --git a/terrain_planner_benchmark/launch/goaltype_benchmark.launch b/terrain_planner_benchmark/launch/goaltype_benchmark.launch index aeb00a9d..72894c9c 100644 --- a/terrain_planner_benchmark/launch/goaltype_benchmark.launch +++ b/terrain_planner_benchmark/launch/goaltype_benchmark.launch @@ -2,14 +2,14 @@ - + - - - - - - + + + + + + diff --git a/terrain_planner_benchmark/package.xml b/terrain_planner_benchmark/package.xml index 8960ab29..4d8e222c 100644 --- a/terrain_planner_benchmark/package.xml +++ b/terrain_planner_benchmark/package.xml @@ -1,4 +1,6 @@ - + + + terrain_planner_benchmark 0.0.1 Benchmarking framework for terrain planners @@ -13,8 +15,7 @@ https://github.com/Jaeyoung-Lim/steep-terrain-navigation/issues - catkin - eigen_catkin + ament_cmake grid_map_core grid_map_cv grid_map_ros @@ -24,8 +25,9 @@ terrain_navigation ompl terrain_planner - eigen_catkin - python3-gdal + python3-gdal + + ament_cmake diff --git a/terrain_planner_benchmark/src/CMakeLists.txt b/terrain_planner_benchmark/src/CMakeLists.txt new file mode 100644 index 00000000..07e7bc9c --- /dev/null +++ b/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 +) diff --git a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp index 233faab3..0c614bb4 100644 --- a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp +++ b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp @@ -38,15 +38,15 @@ * @author Jaeyoung Lim */ -#include -#include #include #include -#include -#include -#include +#include #include +#include +#include +#include +#include #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" @@ -54,29 +54,35 @@ #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("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); - auto path_pub = nh.advertise("path", 1, true); - auto interpolate_path_pub = nh.advertise("interpolated_path", 1, true); - auto path_segment_pub = nh.advertise("path_segments", 1, true); - auto grid_map_pub = nh.advertise("grid_map", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); - std::string map_path, color_file_path, output_file_dir, location; - int number_of_runs; - nh_private.param("map_path", map_path, ""); - nh_private.param("location", location, ""); - nh_private.param("color_file_path", color_file_path, ""); - nh_private.param("output_directory", output_file_dir, ""); - nh_private.param("number_of_runs", number_of_runs, 10); + auto start_pos_pub = + node->create_publisher("start_position", rclcpp::QoS(1).transient_local()); + auto goal_pos_pub = + node->create_publisher("goal_position", rclcpp::QoS(1).transient_local()); + auto path_pub = node->create_publisher("path", rclcpp::QoS(1).transient_local()); + auto interpolate_path_pub = + node->create_publisher("interpolated_path", rclcpp::QoS(1).transient_local()); + auto path_segment_pub = + node->create_publisher("path_segments", rclcpp::QoS(1).transient_local()); + auto grid_map_pub = node->create_publisher("grid_map", rclcpp::QoS(1).transient_local()); + auto trajectory_pub = + node->create_publisher("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(); - 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); } diff --git a/terrain_planner_benchmark/src/test_ics.cpp b/terrain_planner_benchmark/src/test_ics.cpp index 0eea5d2a..45590ba2 100644 --- a/terrain_planner_benchmark/src/test_ics.cpp +++ b/terrain_planner_benchmark/src/test_ics.cpp @@ -164,8 +164,8 @@ int main(int argc, char** argv) { auto grid_map_pub_ = nh.advertise("grid_map", 1, true); auto reference_map_pub_ = nh.advertise("reference_map", 1, true); - auto yaw_pub = nh.advertise("yaw", 1, true); - auto circle_pub = nh.advertise("circle", 1, true); + auto yaw_pub = nh.advertise("yaw", 1, true); + auto circle_pub = nh.advertise("circle", 1, true); std::string map_path, map_color_path, output_file_dir, location; bool visualize{true}; diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp index cedc66fa..4aa177d3 100644 --- a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp +++ b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp @@ -55,10 +55,10 @@ #include "terrain_planner/visualization.h" void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, const double radius) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.stamp = rclcpp::Clock().now(); - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; marker.header.frame_id = "map"; marker.id = 0; marker.header.stamp = rclcpp::Clock().now(); @@ -148,11 +148,11 @@ int main(int argc, char** argv) { ros::NodeHandle nh_private("~"); // Initialize ROS related publishers for visualization - auto start_pos_pub = nh.advertise("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); + auto start_pos_pub = nh.advertise("start_position", 1, true); + auto goal_pos_pub = nh.advertise("goal_position", 1, true); auto path_pub = nh.advertise("path", 1, true); auto grid_map_pub = nh.advertise("grid_map", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); + auto trajectory_pub = nh.advertise("tree", 1, true); std::string map_path, color_file_path, output_directory, location; nh_private.param("map_path", map_path, ""); diff --git a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp index 0a084d1a..1e1003bc 100644 --- a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp +++ b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp @@ -83,11 +83,11 @@ int main(int argc, char** argv) { ros::NodeHandle nh_private("~"); // Initialize ROS related publishers for visualization - auto start_pos_pub = nh.advertise("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); + auto start_pos_pub = nh.advertise("start_position", 1, true); + auto goal_pos_pub = nh.advertise("goal_position", 1, true); auto path_pub = nh.advertise("path", 1, true); auto grid_map_pub = nh.advertise("grid_map", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); + auto trajectory_pub = nh.advertise("tree", 1, true); std::string map_path, color_file_path, location, output_directory; nh_private.param("location", location, "");