Ported to ROS2 Foxy

mpc_local_planner/CMakeLists.txt

@@ -1,37 +1,45 @@
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 3.5)
 project(mpc_local_planner)
 
 # Set to Release in order to speed up the program significantly
 set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+## Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  base_local_planner
-  costmap_2d
-  costmap_converter
-  dynamic_reconfigure
-  roscpp
-  geometry_msgs
-  interactive_markers
-  nav_core
-  nav_msgs
-  mbf_costmap_core
-  mbf_msgs
-  mpc_local_planner_msgs
-  pluginlib
-  std_msgs
-  teb_local_planner
-  tf2
-  tf2_eigen
-  tf2_geometry_msgs
-  tf2_ros
-  visualization_msgs
-)
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util)
+find_package(nav_2d_utils)
+find_package(costmap_converter REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(mpc_local_planner_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(teb_local_planner REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+message(STATUS "System: ${CMAKE_SYSTEM}")
+## System dependencies are found with CMake's conventions
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
+message(STATUS "${CMAKE_MODULE_PATH}")
+
+find_package(G2O REQUIRED)
 
 # Find control_box_rst
 # TODO: Currently, we use QUIET rather than REQUIRED to avoid warnings
@@ -59,100 +67,7 @@ find_package(osqp QUIET)
 
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS}
-  LIBRARIES mpc_local_planner_numerics mpc_local_planner_optimization
-  CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner
-  # DEPENDS
-)
+set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR} ${teb_local_planner_INCLUDE_DIRS} ${G2O_INCLUDE_DIR}/../)
 
 ###########
 ## Build ##
@@ -163,17 +78,16 @@ catkin_package(
 include_directories(
   include
   ${EXTERNAL_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(mpc_local_planner_utils
+add_library(mpc_local_planner_utils SHARED
    src/utils/publisher.cpp
    src/utils/conversion.cpp
    src/utils/time_series_se2.cpp
 )
 
-add_library(mpc_local_planner_optimal_control
+add_library(mpc_local_planner_optimal_control SHARED
    src/optimal_control/full_discretization_grid_base_se2.cpp
    src/optimal_control/finite_differences_grid_se2.cpp
    src/optimal_control/finite_differences_variable_grid_se2.cpp
@@ -183,15 +97,54 @@ add_library(mpc_local_planner_optimal_control
    src/optimal_control/min_time_via_points_cost.cpp
 )
 
-add_library(mpc_local_planner
+add_library(mpc_local_planner SHARED
    src/controller.cpp
    src/mpc_local_planner_ros.cpp
+   src/mpc_config.cpp
+)
+
+set(ament_dependencies
+    builtin_interfaces
+    nav2_core
+    nav2_costmap_2d
+    nav2_util
+    nav_2d_utils
+    costmap_converter
+    rclcpp
+    geometry_msgs
+    interactive_markers
+    nav_msgs
+    mpc_local_planner_msgs
+    pluginlib
+    rosidl_runtime_cpp
+    std_msgs
+    teb_local_planner
+    tf2
+    tf2_eigen
+    tf2_geometry_msgs
+    tf2_ros
+    visualization_msgs
+)
+ament_target_dependencies(mpc_local_planner
+   ${ament_dependencies}
+)
+
+ament_target_dependencies(mpc_local_planner_optimal_control
+   ${ament_dependencies}
+)
+
+ament_target_dependencies(mpc_local_planner_utils
+   ${ament_dependencies}
 )
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(test_mpc_optim_node src/test_mpc_optim_node.cpp)
+#add_executable(test_mpc_optim_node src/test_mpc_optim_node.cpp)
+
+#ament_target_dependencies(test_mpc_optim_node
+#   ${ament_dependencies}
+#)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -217,64 +170,45 @@ target_link_libraries(mpc_local_planner
   mpc_local_planner_utils
   mpc_local_planner_optimal_control
   corbo_controllers
-  ${catkin_LIBRARIES}
 )
 
- target_link_libraries(test_mpc_optim_node
-   mpc_local_planner
-   ${catkin_LIBRARIES}
- )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+# target_link_libraries(test_mpc_optim_node
+#   mpc_local_planner
+# )
 
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
 install(PROGRAMS
   scripts/plot_optimal_control_results.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 ## Mark executables and/or libraries for installation
 install(TARGETS mpc_local_planner mpc_local_planner_utils mpc_local_planner_optimal_control
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
-install(TARGETS test_mpc_optim_node
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+   DESTINATION lib
 )
+#install(TARGETS test_mpc_optim_node
+#   RUNTIME DESTINATION lib/${PROJECT_NAME}
+#)
 
 ## Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-   #FILES_MATCHING PATTERN "*.h"
-   PATTERN ".svn" EXCLUDE
+install(DIRECTORY include/
+   DESTINATION include/
 )
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 install(FILES
   mpc_local_planner_plugin.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share
 )
 
 install(DIRECTORY
   launch cfg scripts
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  PATTERN ".svn" EXCLUDE
+  DESTINATION share/${PROJECT_NAME}
 )
 
-#############
-## Testing ##
-#############
+ament_export_include_directories(include)
+ament_export_libraries(mpc_local_planner)
+ament_export_dependencies(${ament_dependencies})
+pluginlib_export_plugin_description_file(nav2_core mpc_local_planner_plugin.xml)
 
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+ament_package()
 
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)