Porting to ROS2 Humble

CMakeLists.txt

@@ -1,42 +1,19 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(hdl_localization)
 
 if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
-  add_definitions(-std=c++11)
-  set(CMAKE_CXX_FLAGS "-std=c++11")
+  add_definitions(-std=c++17)
+  set(CMAKE_CXX_FLAGS "-std=c++17")
 else()
 	# -mavx causes a lot of errors!!
-  if("$ENV{ROS_DISTRO}" STRGREATER "melodic")
-    add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
-    set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-  else()
-    add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
-    set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-  endif()
+  add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+  set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
 endif()
 
-# pcl 1.7 causes a segfault when it is built with debug mode
 set(CMAKE_BUILD_TYPE "RELEASE")
 
-find_package(catkin REQUIRED COMPONENTS
-  nodelet
-  tf2
-  tf2_ros
-  tf2_eigen
-  tf2_geometry_msgs
-  eigen_conversions
-  pcl_ros
-  roscpp
-  rospy
-  sensor_msgs
-  geometry_msgs
-  message_generation
-  ndt_omp
-  fast_gicp
-  hdl_global_localization
-)
+find_package(PCL REQUIRED)
 
-find_package(PCL 1.7 REQUIRED)
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
@@ -51,47 +28,125 @@ if (OPENMP_FOUND)
     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 endif()
 
-########################
-## message generation ##
-########################
-add_message_files(FILES
-  ScanMatchingStatus.msg
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(tf2_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(ndt_omp REQUIRED)
+find_package(fast_gicp REQUIRED)
+find_package(hdl_global_localization REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ScanMatchingStatus.msg"
+  DEPENDENCIES std_msgs geometry_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+rosidl_get_typesupport_target(cpp_typesupport_target
+  ${PROJECT_NAME} rosidl_typesupport_cpp)
+
+add_library(hdl_localization_component
+  src/hdl_localization/pose_estimator.cpp
+  apps/hdl_localization.cpp
+)
+target_include_directories(hdl_localization_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(hdl_localization_component
+  tf2_ros 
+  tf2_eigen 
+  pcl_ros 
+  rclcpp 
+  sensor_msgs 
+  geometry_msgs 
+  ndt_omp 
+  fast_gicp 
+  hdl_global_localization
+  std_srvs
+  nav_msgs
+  rclcpp_components
+  std_msgs
 )
-generate_messages(DEPENDENCIES std_msgs geometry_msgs)
-
-###################################
-## catkin specific configuration ##
-###################################
-catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES hdl_scan_matching_odometry
-#  CATKIN_DEPENDS pcl_ros roscpp sensor_msgs
-#  DEPENDS system_lib
+target_link_libraries(hdl_localization_component
+  ${PCL_LIBRARIES}  "${cpp_typesupport_target}"
 )
 
-###########
-## Build ##
-###########
-include_directories(include)
-include_directories(
-  ${catkin_INCLUDE_DIRS}
+rclcpp_components_register_node(
+    hdl_localization_component
+    PLUGIN "hdl_localization::HdlLocalization"
+    EXECUTABLE hdl_localization_node
 )
 
-# nodelets
-add_library(hdl_localization_nodelet
-  src/hdl_localization/pose_estimator.cpp
-  apps/hdl_localization_nodelet.cpp
+
+add_library(globalmap_server_component
+  apps/globalmap_server.cpp
+)
+target_include_directories(globalmap_server_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(globalmap_server_component
+  tf2_ros 
+  tf2_eigen
+  pcl_ros 
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+  ndt_omp 
+  fast_gicp 
+  hdl_global_localization
+  std_srvs
+  nav_msgs
+  rclcpp_components
+  std_msgs
 )
-target_link_libraries(hdl_localization_nodelet
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
+target_link_libraries(globalmap_server_component
+  ${PCL_LIBRARIES}  "${cpp_typesupport_target}"
 )
-add_dependencies(hdl_localization_nodelet ${PROJECT_NAME}_gencpp)
 
+rclcpp_components_register_node(
+    globalmap_server_component
+    PLUGIN "hdl_localization::GlobalmapServer"
+    EXECUTABLE globalmap_server
+)
+
+
+install(DIRECTORY launch rviz param
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_targets(export_hdl_localization_component)
+install(TARGETS hdl_localization_component
+        EXPORT export_hdl_localization_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+ament_export_targets(export_globalmap_server_component)
+install(TARGETS globalmap_server_component
+        EXPORT export_globalmap_server_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
 
-add_library(globalmap_server_nodelet apps/globalmap_server_nodelet.cpp)
-target_link_libraries(globalmap_server_nodelet
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
+install(PROGRAMS
+  scripts/plot_status.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
+ament_package()

README.md

@@ -25,17 +25,17 @@ The following ros packages are required:
 ## Installation
 
 ```bash
-cd /your/catkin_ws/src
+cd /your/ros2_ws/src
 git clone https://github.com/koide3/ndt_omp
 git clone https://github.com/SMRT-AIST/fast_gicp --recursive
-git clone https://github.com/koide3/hdl_localization
-git clone https://github.com/koide3/hdl_global_localization
+git clone https://github.com/koide3/hdl_localization -b humble
+git clone https://github.com/koide3/hdl_global_localization -b humble --recursive
 
-cd /your/catkin_ws
-catkin_make -DCMAKE_BUILD_TYPE=Release
+cd /your/ros2_ws
+colcon build --cmake-args "-DCMAKE_BUILD_TYPE=Release"
 
 # if you want to enable CUDA-accelerated NDT
-# catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON
+# colcon build --cmake-args "-DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON"
 ```
 
 ### Support docker :whale:  
@@ -46,19 +46,19 @@ Please refer to the repository below and use the docker easily.
 - [Taeyoung96/hdl_localization_tutorial](https://github.com/Taeyoung96/hdl_localization_tutorial)
 
 ## Parameters
-All configurable parameters are listed in *launch/hdl_localization.launch* as ros params.
+All configurable parameters are listed in *param/hdl_localization.yaml* and *launch/hdl_localization.launch.py* as ros parameters.
 The estimated pose can be reset using using "2D Pose Estimate" on rviz
 
 ## Topics
-- ***/odom*** (nav_msgs/Odometry)
+- ***/odom*** (nav_msgs/msg/Odometry)
   - Estimated sensor pose in the map frame
 - ***/aligned_points***
   - Input point cloud aligned with the map
-- ***/status*** (hdl_localization/ScanMatchingStatus)
+- ***/status*** (hdl_localization/msg/ScanMatchingStatus)
   - Scan matching result information (e.g., convergence, matching error, and inlier fraction)
 
 ## Services
-- ***/relocalize*** (std_srvs/Empty)
+- ***/relocalize*** (std_srvs/srv/Empty)
   - Reset the sensor pose with the global localization result
   - For details of the global localization method, see [hdl_global_localization](https://github.com/koide3/hdl_global_localization)
 
@@ -67,22 +67,24 @@ The estimated pose can be reset using using "2D Pose Estimate" on rviz
 Example bag file (recorded in an outdoor environment): [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (933MB)
 
 ```bash
-rosparam set use_sim_time true
-roslaunch hdl_localization hdl_localization.launch
+ros2 launch hdl_localization hdl_localization.launch.py
+ros2 param set use_sim_time true
 ```
 
 ```bash
-roscd hdl_localization/rviz
-rviz -d hdl_localization.rviz
+colcon_cd hdl_localization
+cd rviz
+rviz2 -d hdl_localization.rviz
 ```
 
 ```bash
-rosbag play --clock hdl_400.bag
+# need rosbag_v2 plugin
+ros2 bag play -s rosbag_v2 --clock hdl_400.bag
 ```
 
 ```bash
 # perform global localization
-rosservice call /relocalize
+ros2 service call /relocalize std_srvs/srv/Empty
 ```
 
 <img src="data/figs/localization1.png" height="256pix" /> <img src="data/figs/localization2.png" height="256pix" />