cmake_minimum_required(VERSION 3.8)
project(obstacle_nav2)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

find_package(ament_cmake REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(obstacle_scanner REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(rclcpp_action REQUIRED)

add_library(nav2_profile_loader
  src/nav2_profile_loader.cpp
)
target_include_directories(nav2_profile_loader PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_link_libraries(nav2_profile_loader yaml-cpp)
ament_target_dependencies(nav2_profile_loader
  rclcpp
)

add_library(trajectory_guard_lib
  src/trajectory_guard.cpp
)
target_include_directories(trajectory_guard_lib PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
ament_target_dependencies(trajectory_guard_lib
  geometry_msgs
  nav_msgs
)

add_library(obstacle_array_layer SHARED
  src/obstacle_array_layer.cpp
)
target_include_directories(obstacle_array_layer PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
ament_target_dependencies(obstacle_array_layer
  nav2_costmap_2d
  pluginlib
  rclcpp
  tf2
  tf2_ros
  tf2_geometry_msgs
  obstacle_scanner
  geometry_msgs
  nav_msgs
  std_msgs
)

pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml)

add_executable(nav2_profile_tuner
  src/nav2_profile_tuner.cpp
)
target_link_libraries(nav2_profile_tuner nav2_profile_loader)
ament_target_dependencies(nav2_profile_tuner
  rclcpp
  rcl_interfaces
  std_msgs
)

add_executable(trajectory_guard_node
  src/trajectory_guard_node.cpp
)
target_include_directories(trajectory_guard_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_link_libraries(trajectory_guard_node trajectory_guard_lib)
ament_target_dependencies(trajectory_guard_node
  geometry_msgs
  nav2_msgs
  nav_msgs
  rclcpp
  rclcpp_action
  tf2
)

install(TARGETS
  nav2_profile_loader
  trajectory_guard_lib
  obstacle_array_layer
  nav2_profile_tuner
  trajectory_guard_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/ DESTINATION include)
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})

# Python scripts
install(PROGRAMS
  scripts/initial_pose_to_tf.py
  scripts/static_map_publisher.py
  DESTINATION share/${PROJECT_NAME}/scripts
)

if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
    src/obstacle_array_layer.cpp
  )
  target_include_directories(test_obstacle_array_layer PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  )
  ament_target_dependencies(test_obstacle_array_layer
    nav2_costmap_2d
    pluginlib
    rclcpp
    tf2
    tf2_ros
    tf2_geometry_msgs
    obstacle_scanner
    geometry_msgs
    nav_msgs
    std_msgs
  )

  ament_add_gtest(test_nav2_profile_loader test/test_nav2_profile_loader.cpp
    src/nav2_profile_loader.cpp
  )
  target_include_directories(test_nav2_profile_loader PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  )
  target_link_libraries(test_nav2_profile_loader yaml-cpp)
  ament_target_dependencies(test_nav2_profile_loader
    rclcpp
  )

  ament_add_gtest(test_trajectory_guard test/test_trajectory_guard.cpp)
  target_include_directories(test_trajectory_guard PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  )
  target_link_libraries(test_trajectory_guard trajectory_guard_lib)
  ament_target_dependencies(test_trajectory_guard
    geometry_msgs
    nav_msgs
  )
endif()

install(DIRECTORY config launch behavior_tree
  DESTINATION share/${PROJECT_NAME}
)

ament_package()
