Port ndt_omp to ROS2 (#20)

* Remove COLCON_IGNORE

* CMakeLists.txt & package.xml

* Compiles

* Remove rclcpp

* Cleanup

* Fix package.xml

* Better CMakeLists.txt

* Update CMakeLists.txt again

* Update CMakeLists.txt

* Update CMakeLists.txt yet again

* Simplify CMakeLists.txt

* Last (?) CMakeLists.txt change

localization/ndt_omp/CMakeLists.txt

@@ -1,70 +1,111 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.9)
 project(ndt_omp)
 
-# -mavx causes a lot of errors!!
-add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
-set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-
-# warn performance issue of ndt_omp which is NOT built in release mode
-string(TOUPPER ${CMAKE_BUILD_TYPE} uppercase_CMAKE_BUILD_TYPE)
-if (NOT (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE"))
-  message(WARNING
-    "CMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} may cause a serious performance degradation. Please configure with -DCMAKE_BUILD_TYPE=Release.")
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wno-unused-parameter)
 endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  pcl_ros
-)
+set(CMAKE_BUILD_TYPE "Release")
 
+find_package(ament_cmake REQUIRED)
+find_package(PCL REQUIRED COMPONENTS common filters kdtree registration io)
 find_package(OpenMP)
-if (OPENMP_FOUND)
-  set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-  set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-endif()
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES ndt_omp
-  CATKIN_DEPENDS
-)
 
 ###########
 ## Build ##
 ###########
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
 add_library(ndt_omp
   src/ndt_omp/voxel_grid_covariance_omp.cpp
   src/ndt_omp/ndt_omp.cpp
 )
 
-add_executable(align
-  apps/align.cpp
-)
-add_dependencies(align
-  ndt_omp
+target_include_directories(ndt_omp
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include> 
 )
-# pcl 1.7 causes a segfault without -O2 (or -O3) flag
-target_compile_options(ndt_omp PUBLIC
-  $<$<CONFIG:Debug>:-O2 -g>
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  target_compile_options(ndt_omp
+    PRIVATE
+      # -mavx causes a lot of errors!!
+      -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2
+  )
+endif()
+
+set(NDT_OMP_DEPENDENCIES
+  PCL
+  OpenMP
 )
-target_link_libraries(align
-  ${catkin_LIBRARIES}
-  ndt_omp
+
+
+# Naively, I'd want to do something like:
+# ament_target_dependencies(ndt_omp PUBLIC ${NDT_OMP_DEPENDENCIES})
+# But there are several problems with that. For one, you'll have a hundred
+# errors like "undefined reference to `pcl::something::Something`". That's
+# because it only links `libpcl_common.so` for whatever reason, but we also
+# want `libpcl_io.so` etc. So we do this PCL thing manually:
+target_compile_definitions(ndt_omp PUBLIC ${PCL_DEFINITIONS})
+target_include_directories(ndt_omp PUBLIC ${PCL_INCLUDE_DIRS})
+target_link_libraries(ndt_omp PUBLIC ${PCL_LIBRARIES})
+target_link_directories(ndt_omp PUBLIC ${PCL_LIBRARY_DIRS})
+
+# As for OpenMP, the recommended format is the following, which is not
+# supported by `ament_target_dependencies()` either.
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(ndt_omp PUBLIC OpenMP::OpenMP_CXX)
+else()
+  message(WARNING "OpenMP not found")
+endif()
+# See https://cliutils.gitlab.io/modern-cmake/chapters/packages/OpenMP.html
+# but it doesn't matter anyway because at the time of writing, omp.h is in
+# /usr/lib/gcc/x86_64-linux-gnu/9/include/ which is one of the default
+# include directories, and libgomp.so is in /usr/lib/x86_64-linux-gnu/,
+# which is one of the default linker search paths.
+
+ament_export_targets(export_ndt_omp HAS_LIBRARY_TARGET)
+ament_export_dependencies(${NDT_OMP_DEPENDENCIES})
+
+
+# Same story for the executable
+add_executable(align
+  apps/align.cpp
 )
 
-install(TARGETS ndt_omp
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+target_compile_definitions(align PRIVATE ${PCL_DEFINITIONS})
+target_include_directories(align PRIVATE ${PCL_INCLUDE_DIRS})
+target_link_libraries(align PRIVATE ${PCL_LIBRARIES})
+target_link_directories(align PRIVATE ${PCL_LIBRARY_DIRS})
+if(OpenMP_CXX_FOUND)
+  target_link_libraries(align PRIVATE OpenMP::OpenMP_CXX)
+else()
+  message(WARNING "OpenMP not found")
+endif()
+
+# Again, no chance for `ament_target_dependencies()` since you can't depend on
+# a local library target: "ament_target_dependencies() the passed package name
+# 'ndt_omp' was not found" – so we need to add ndt_omp with
+# `target_link_libraries(align PUBLIC ndt_omp)` instead.
+target_link_libraries(align PRIVATE ndt_omp)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
 )
 
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(
+  TARGETS ndt_omp
+  EXPORT export_ndt_omp
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
+
+ament_package()
+

localization/ndt_omp/apps/align.cpp

@@ -26,13 +26,13 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  * 
 */
+#include <iostream>
+#include <omp.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/registration/ndt.h>
-#include <ros/ros.h>
-#include <iostream>
 
 #include <ndt_omp/ndt_omp.h>
 
@@ -45,17 +45,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr align(
   registration->setInputTarget(target_cloud);
   registration->setInputSource(source_cloud);
   pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
-
-  auto t1 = ros::WallTime::now();
+  auto t1 = std::chrono::steady_clock::now();
   registration->align(*aligned);
-  auto t2 = ros::WallTime::now();
-  std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl;
+  auto t2 = std::chrono::steady_clock::now();
+  std::cout << "single : " << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << "[msec]" << std::endl;
 
   for (int i = 0; i < 10; i++) {
     registration->align(*aligned);
   }
-  auto t3 = ros::WallTime::now();
-  std::cout << "10times: " << (t3 - t2).toSec() * 1000 << "[msec]" << std::endl;
+  auto t3 = std::chrono::steady_clock::now();
+  std::cout << "10times: " << std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2).count() << "[msec]" << std::endl;
   std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl;
 
   return aligned;
@@ -97,8 +96,6 @@ int main(int argc, char ** argv)
   voxelgrid.filter(*downsampled);
   source_cloud = downsampled;
 
-  ros::Time::init();
-
   // benchmark
   std::cout << "--- pcl::NDT ---" << std::endl;
   pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt(

localization/ndt_omp/package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> 
+<package format="3">
   <name>ndt_omp</name>
   <version>0.1.0</version>
   <description>OpenMP boosted NDT and GICP algorithms</description>
@@ -11,12 +12,10 @@
 
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>pcl_ros</build_depend>
-  <build_depend>roscpp</build_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>roscpp</run_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>libpcl-all-dev</depend>
 
   <export>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>