Port terrain-navigation to ROS 2 Humble

planner_msgs/CMakeLists.txt

@@ -1,27 +1,23 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.14.4)
 project(planner_msgs)
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs)
+find_package(ament_cmake REQUIRED)
 
-include_directories(include)
+find_package(Eigen3 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-add_message_files(
-  DIRECTORY msg
-  FILES
-  NavigationStatus.msg
-  TerrainInfo.msg
+rosidl_generate_interfaces(${PROJECT_NAME}
+  msg/NavigationStatus.msg
+  msg/TerrainInfo.msg
+  srv/SetPlannerState.srv
+  srv/SetService.srv
+  srv/SetString.srv
+  srv/SetVector3.srv
+  DEPENDENCIES std_msgs geometry_msgs
 )
 
-add_service_files(
-  FILES
-  SetString.srv
-  SetVector3.srv
-  SetService.srv
-  SetPlannerState.srv
-)
-
-generate_messages(DEPENDENCIES geometry_msgs std_msgs)
+ament_export_dependencies(rosidl_default_runtime)
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS geometry_msgs message_runtime std_msgs)
+ament_package()

planner_msgs/package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>planner_msgs</name>
   <version>0.0.1</version>
   <description>
@@ -10,18 +11,16 @@
   <author email="[email protected]">Jaeyoung Lim</author>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>message_generation</build_depend>
-  <exec_depend>message_runtime</exec_depend>
-  <depend>std_msgs</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
-  <depend>mavros_msgs</depend>
-  <depend>sensor_msgs</depend>
-  <!-- XXX needed for users of mavlink_convert.h
-  <build_export_depend>libmavconn</build_export_depend>
-  -->
+  <depend>std_msgs</depend>
+  <depend>rosidl_default_generators</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
   <export>
-    <architecture_independent/>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

terrain_navigation/CMakeLists.txt

@@ -1,43 +1,104 @@
-cmake_minimum_required(VERSION 2.8.12)
+cmake_minimum_required(VERSION 3.14.4)
 project(terrain_navigation)
-add_definitions(-std=c++17)
 
-find_package(catkin REQUIRED COMPONENTS
-    eigen_catkin
-    grid_map_core
-    grid_map_geo
-)
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES terrain_navigation
-)
+# Set policy for 3.16 for CMP0076 defaulting to ON
+cmake_policy(VERSION 3.16)
 
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-  include
-  ${Boost_INCLUDE_DIR}
-  ${Eigen_INCLUDE_DIRS}
-)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(grid_map_geo REQUIRED)
+
+find_package(class_loader REQUIRED)
+find_package(console_bridge REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rosbag2_storage REQUIRED)
+find_package(tinyxml2 REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+find_package(action_msgs REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+find_package(GDAL 3 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+find_package(yaml_cpp_vendor REQUIRED)
+link_directories(${yaml_cpp_vendor_LIBRARY_DIRS})
+
+# Reverse compatability for GDAL<3.5
+# https://gdal.org/development/cmake.html
+if(NOT TARGET GDAL::GDAL)
+  add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY})
+endif()
 
 add_library(${PROJECT_NAME}
   src/data_logger.cpp
   src/profiler.cpp
   src/terrain_map.cpp
 )
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
 
-if(CATKIN_ENABLE_TESTING)
-    # Add gtest based cpp test target and link libraries
-    catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp
-                                          test/test_trajectory.cpp)
-                                          # test/test_terrain_map.cpp)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Eigen3::Eigen
+  GDAL::GDAL
+  ${OpenCV_LIBRARIES}
+)
+
+ament_target_dependencies(${PROJECT_NAME} PUBLIC
+  grid_map_core
+  grid_map_geo
+)
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+  ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(GDAL)
 
-    if(TARGET ${PROJECT_NAME}-test)
-        target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}
-                                                 ${catkin_LIBRARIES}
-                                                 ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES})
-    endif()
+# Test
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}-test
+    test/main.cpp
+    test/test_trajectory.cpp
+    # test/test_terrain_map.cpp
+  )
 
+  target_link_libraries(${PROJECT_NAME}-test
+    ${PROJECT_NAME}
+    ${OpenCV_LIBRARIES}
+  )
 endif()
+
+ament_package()

terrain_navigation/include/terrain_navigation/data_logger.h

@@ -62,7 +62,8 @@ class DataLogger {
   void writeToFile(const std::string path);
 
  private:
-  int id_{0};
+  //! @todo(srmainwaring) remove unused variable
+  // int id_{0};
   std::vector<std::string> keys_;
   std::vector<std::unordered_map<std::string, std::any>> data_list_;
   bool print_header_{false};

terrain_navigation/include/terrain_navigation/path.h

@@ -70,7 +70,7 @@ class Path {
   void prependSegment(const PathSegment &trajectory) { segments.insert(segments.begin(), trajectory); };
   void appendSegment(const PathSegment &trajectory) { segments.push_back(trajectory); };
   void appendSegment(const Path &trajectory_segments) {
-    for (const auto trajectory : trajectory_segments.segments) {
+    for (const auto& trajectory : trajectory_segments.segments) {
       appendSegment(trajectory);
     }
   };
@@ -178,7 +178,7 @@ class Path {
    */
   double getLength(const int start_idx = 0) {
     double length{0.0};
-    for (int i = start_idx; i < segments.size(); i++) {
+    for (size_t i = start_idx; i < segments.size(); i++) {
       length += segments[i].getLength();
     }
     return length;

terrain_navigation/include/terrain_navigation/path_segment.h

@@ -36,11 +36,12 @@
 #ifndef PATH_SEGMENT_H
 #define PATH_SEGMENT_H
 
-#include <Eigen/Dense>
 #include <iostream>
 #include <memory>
 #include <vector>
 
+#include <Eigen/Dense>
+
 struct State {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   Eigen::Vector3d position;

terrain_navigation/include/terrain_navigation/primitive.h

@@ -72,7 +72,8 @@ class Primitive {
   std::vector<Path> getMotionPrimitives() {
     std::vector<Path> all_primitives;
     if (has_child()) {
-      int i = 0;
+      //! @todo(srmainwaring) remove set but unused variable
+      // int i = 0;
       for (const auto &child : child_primitives) {
         std::vector<Path> extended_primitives = child->getMotionPrimitives();
         // Append current segment
@@ -81,7 +82,8 @@ class Primitive {
           primitive.validity = validity && primitive.validity;
           all_primitives.push_back(primitive);
         }
-        i++;
+        //! @todo(srmainwaring) remove set but unused variable
+        // i++;
       }
     } else {  // Append primitive segments
       Path trajectory_segments;
@@ -102,6 +104,7 @@ class Primitive {
     for (auto &child : child_primitives) {
       if (child->valid()) return child;
     }
+    return nullptr;
   }
 
   std::shared_ptr<Primitive> copy() const {

terrain_navigation/include/terrain_navigation/terrain_map.h

@@ -36,17 +36,26 @@
 #ifndef TERRAIN_MAP_H
 #define TERRAIN_MAP_H
 
-#include <grid_map_geo/grid_map_geo.h>
+#include <iostream>
+#include <memory>
+
+#include <grid_map_geo/grid_map_geo.hpp>
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
+#if __APPLE__
+#include <cpl_string.h>
+#include <gdal.h>
+#include <gdal_priv.h>
+#include <ogr_p.h>
+#include <ogr_spatialref.h>
+#else
 #include <gdal/cpl_string.h>
 #include <gdal/gdal.h>
 #include <gdal/gdal_priv.h>
 #include <gdal/ogr_p.h>
 #include <gdal/ogr_spatialref.h>
-#include <iostream>
-#include <memory>
+#endif
 
 class TerrainMap : public GridMapGeo {
  public:

terrain_navigation/include/terrain_navigation/viewpoint.h

@@ -41,14 +41,16 @@
 #ifndef TERRAIN_PLANNER_VIEWPOINT_H
 #define TERRAIN_PLANNER_VIEWPOINT_H
 
-#include <Eigen/Dense>
 #include <iostream>
+#include <vector>
+
+#include <Eigen/Dense>
+
 #include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-#include <vector>
-#include "opencv2/core.hpp"
 
 class ViewPoint {
  public:

terrain_navigation/include/terrain_navigation/visualization.h

@@ -42,30 +42,31 @@
 #ifndef VISUALIZATION_H
 #define VISUALIZATION_H
 
-#include "geometry_msgs/Point.h"
-#include "terrain_navigation/viewpoint.h"
-#include "visualization_msgs/Marker.h"
+#include <rclcpp/clock.hpp>
 
-geometry_msgs::Point toPoint(const Eigen::Vector3d &p) {
-  geometry_msgs::Point position;
+#include <geometry_msgs/msg/point.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) {
+  geometry_msgs::msg::Point position;
   position.x = p(0);
   position.y = p(1);
   position.z = p(2);
   return position;
 }
 
-visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint,
+visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint,
                                                Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) {
   double scale{15};  // Size of the viewpoint markers
-  visualization_msgs::Marker marker;
+  visualization_msgs::msg::Marker marker;
   marker.header.frame_id = "map";
-  marker.header.stamp = ros::Time();
+  marker.header.stamp = rclcpp::Clock().now();
   marker.ns = "my_namespace";
   marker.id = id;
-  marker.type = visualization_msgs::Marker::LINE_LIST;
-  marker.action = visualization_msgs::Marker::ADD;
+  marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+  marker.action = visualization_msgs::msg::Marker::ADD;
   const Eigen::Vector3d position = viewpoint.getCenterLocal();
-  std::vector<geometry_msgs::Point> points;
+  std::vector<geometry_msgs::msg::Point> points;
   std::vector<Eigen::Vector3d> corner_ray_vectors = viewpoint.getCornerRayVectors();
   std::vector<Eigen::Vector3d> vertex;
   for (auto &corner_ray : corner_ray_vectors) {