Switch nav2_amcl to use modern CMake idioms.

This change is a little unusual in that it does rearrange
the hierarchy of CMakeLists.txt files.  This is because
the individual libraries (like map_lib) are used in some of
the other libraries, and CMake was complaining about that.
So I flattened the whole thing out and now build all of the
libraries at the top-level.

This PR also contains a couple of fixes to previous packages
where I failed to properly export the include directory.

Finally, this PR goes ahead and removes quite a bit of dead
code from this package.  This has been dead for quite a while,
so I think it makes sense to remove it at this point.

Signed-off-by: Chris Lalancette <[email protected]>

nav2_amcl/CMakeLists.txt

@@ -2,90 +2,136 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
 find_package(rclcpp_components REQUIRED)
-find_package(message_filters REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(nav2_util REQUIRED)
-find_package(nav2_msgs REQUIRED)
-find_package(pluginlib REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(
-  include
-)
-
 include(CheckSymbolExists)
 check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
 
-add_subdirectory(src/pf)
-add_subdirectory(src/map)
-add_subdirectory(src/motion_model)
-add_subdirectory(src/sensors)
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-gnu-folding-constant)
+endif()
 
-set(executable_name amcl)
+add_library(pf_lib SHARED
+  src/pf/pf.c
+  src/pf/pf_kdtree.c
+  src/pf/pf_pdf.c
+  src/pf/pf_vector.c
+  src/pf/eig3.c
+  src/pf/pf_draw.c
+)
+target_include_directories(pf_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+if(HAVE_DRAND48)
+  target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
+endif()
 
-add_executable(${executable_name}
-  src/main.cpp
+add_library(map_lib SHARED
+  src/map/map.c
+  src/map/map_range.c
+  src/map/map_draw.c
+  src/map/map_cspace.cpp
+)
+target_include_directories(map_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+
+add_library(motions_lib SHARED
+  src/motion_model/omni_motion_model.cpp
+  src/motion_model/differential_motion_model.cpp
+)
+target_include_directories(motions_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(motions_lib PUBLIC
+  pf_lib
+  pluginlib::pluginlib
+  nav2_util::nav2_util_core
+)
+
+add_library(sensors_lib SHARED
+  src/sensors/laser/laser.cpp
+  src/sensors/laser/beam_model.cpp
+  src/sensors/laser/likelihood_field_model.cpp
+  src/sensors/laser/likelihood_field_model_prob.cpp
+)
+target_include_directories(sensors_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(sensors_lib PUBLIC
+  pf_lib
+  map_lib
 )
 
+set(executable_name amcl)
+
 set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
   src/amcl_node.cpp
 )
-
-target_include_directories(${library_name} PRIVATE src/include)
-
 if(HAVE_DRAND48)
   target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
 endif()
-
-set(dependencies
-  rclcpp
-  rclcpp_lifecycle
-  rclcpp_components
-  message_filters
-  tf2_geometry_msgs
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  std_srvs
-  tf2_ros
-  tf2
-  nav2_util
-  nav2_msgs
-  pluginlib
+target_include_directories(${library_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  message_filters::message_filters
+  nav2_util::nav2_util_core
+  ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
+  pluginlib::pluginlib
+  sensors_lib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${nav_msgs_TARGETS}
+  tf2_ros::tf2_ros
+  tf2::tf2
+  ${nav2_msgs_TARGETS}
 )
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
+target_link_libraries(${library_name} PRIVATE
+  rclcpp_components::component
+  tf2_geometry_msgs::tf2_geometry_msgs
 )
 
-target_link_libraries(${executable_name}
-  ${library_name}
-)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+add_executable(${executable_name}
+  src/main.cpp
 )
-
-target_link_libraries(${library_name}
-  map_lib pf_lib sensors_lib
+target_include_directories(${executable_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${executable_name} PRIVATE
+  ${library_name}
 )
 
 rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
 
-install(TARGETS ${library_name}
+install(TARGETS ${library_name} pf_lib map_lib motions_lib sensors_lib
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -96,19 +142,34 @@ install(TARGETS ${executable_name}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
+
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  message_filters
+  nav_msgs
+  nav2_msgs
+  nav2_util
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  sensor_msgs
+  std_srvs
+  tf2
+  tf2_ros
+)
+ament_export_targets(${library_name})
 pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)
 ament_package()

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -37,17 +37,13 @@
 #include "nav2_msgs/msg/particle_cloud.hpp"
 #include "nav2_msgs/srv/set_initial_pose.hpp"
 #include "nav_msgs/srv/set_map.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
-#include "pluginlib/class_loader.hpp"
-
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-parameter"
-#pragma GCC diagnostic ignored "-Wreorder"
-#include "tf2_ros/message_filter.h"
-#pragma GCC diagnostic pop
 
 #define NEW_UNIFORM_SAMPLING 1
 

nav2_amcl/include/nav2_amcl/angleutils.hpp

@@ -22,7 +22,7 @@
 #ifndef NAV2_AMCL__ANGLEUTILS_HPP_
 #define NAV2_AMCL__ANGLEUTILS_HPP_
 
-#include <math.h>
+#include <cmath>
 
 namespace nav2_amcl
 {

nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp

@@ -22,11 +22,9 @@
 #ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
 #define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
 
-#include <sys/types.h>
-#include <math.h>
-#include <algorithm>
 #include "nav2_amcl/motion_model/motion_model.hpp"
-#include "nav2_amcl/angleutils.hpp"
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_vector.hpp"
 
 
 namespace nav2_amcl

nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

@@ -17,11 +17,8 @@
 #ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
 #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
 
-#include <string>
-#include <memory>
-
 #include "nav2_amcl/pf/pf.hpp"
-#include "nav2_amcl/pf/pf_pdf.hpp"
+#include "nav2_amcl/pf/pf_vector.hpp"
 
 namespace nav2_amcl
 {

nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp

@@ -22,12 +22,9 @@
 #ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
 #define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
 
-#include <sys/types.h>
-#include <math.h>
-#include <algorithm>
 #include "nav2_amcl/motion_model/motion_model.hpp"
-#include "nav2_amcl/angleutils.hpp"
-
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_vector.hpp"
 
 namespace nav2_amcl
 {

nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp

@@ -30,9 +30,6 @@
 
 #include "nav2_amcl/pf/pf_vector.hpp"
 
-// #include <gsl/gsl_rng.h>
-// #include <gsl/gsl_randist.h>
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -65,9 +62,6 @@ pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
 // Destroy the pdf
 void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf);
 
-// Compute the value of the pdf at some point [z].
-// double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
-
 // Draw randomly from a zero-mean Gaussian distribution, with standard
 // deviation sigma.
 // We use the polar form of the Box-Muller transformation, explained here:

nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp

@@ -51,38 +51,15 @@ typedef struct
 // Return a zero vector
 pf_vector_t pf_vector_zero(void);
 
-// Check for NAN or INF in any component
-// int pf_vector_finite(pf_vector_t a);
-
-// Print a vector
-// void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt);
-
-// Simple vector addition
-// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
-
 // Simple vector subtraction
 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
 
 // Transform from local to global coords (a + b)
 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
 
-// Transform from global to local coords (a - b)
-// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
-
-
 // Return a zero matrix
 pf_matrix_t pf_matrix_zero(void);
 
-// Check for NAN or INF in any component
-// int pf_matrix_finite(pf_matrix_t a);
-
-// Print a matrix
-// void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt);
-
-// Compute the matrix inverse.  Will also return the determinant,
-// which should be checked for underflow (indicated singular matrix).
-// pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
-
 // Decompose a covariance matrix [a] into a rotation matrix [r] and a
 // diagonal matrix [d] such that a = r * d * r^T.
 void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a);

nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

@@ -18,10 +18,10 @@
 #ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
 #define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
 
-#include <string>
+#include "nav2_amcl/map/map.hpp"
 #include "nav2_amcl/pf/pf.hpp"
 #include "nav2_amcl/pf/pf_pdf.hpp"
-#include "nav2_amcl/map/map.hpp"
+#include "nav2_amcl/pf/pf_vector.hpp"
 
 namespace nav2_amcl
 {