cmake_minimum_required(VERSION 3.8)
project(rbs_skill_servers)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package(Eigen3 3.3 REQUIRED)
find_package(nlohmann_json 3.2.0 REQUIRED)
find_package(rbs_utils REQUIRED)

# Default to Fortress
set(SDF_VER 12)

# If the user didn't specify a GZ distribution, pick the one matching the ROS
# distribution according to REP 2000
if(NOT DEFINED ENV{GZ_VERSION} AND DEFINED ENV{ROS_DISTRO})
  if("$ENV{ROS_DISTRO}" STREQUAL "humble")
    set(ENV{GZ_VERSION} "fortress")
  endif()
endif()

# Find libsdformat matching the picked GZ distribution
if("$ENV{GZ_VERSION}" STREQUAL "fortress")
  find_package(sdformat12 REQUIRED)
  set(SDF_VER ${sdformat12_VERSION_MAJOR})
  message(STATUS "Compiling against Gazebo Fortress (libSDFormat 12)")
elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
  find_package(sdformat13 REQUIRED)
  set(SDF_VER ${sdformat13_VERSION_MAJOR})
  message(STATUS "Compiling against Gazebo Garden (libSDFormat 13)")
  # No GZ distribution specified, find any version of libsdformat we can
else()
  foreach(major RANGE 13 9)
    find_package(sdformat${major} QUIET)
    if(sdformat${major}_FOUND)
      # Next `find_package` call will be a noop
      set(SDF_VER ${major})
      message(STATUS "Compiling against libSDFormat ${major}")
      break()
    endif()
  endforeach()
endif()

include_directories(SYSTEM ${TINYXML2_INCLUDE_DIR})

link_directories(${TINYXML2_LIBRARY_DIRS})

set(deps
    rclcpp
    rclcpp_action
    moveit_core
    moveit_ros_planning
    moveit_ros_planning_interface
    moveit_msgs
    geometry_msgs
    tf2_ros
    rclcpp_components
    rbs_skill_interfaces
    tf2_eigen
    tf2_msgs
    tinyxml2_vendor
    geometric_shapes
    sdformat${SDF_VER})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

# -- GripperActionServer --
add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
target_include_directories(
  gripper_action_server
  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
          $<INSTALL_INTERFACE:include>)
target_compile_definitions(gripper_action_server
                           PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
ament_target_dependencies(gripper_action_server ${deps})
rclcpp_components_register_node(
  gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer"
  EXECUTABLE gripper_control_action_server)

# -- PickPlacePoseLoader --
add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
target_include_directories(
  pick_place_pose_loader
  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
          $<INSTALL_INTERFACE:include>)
target_compile_definitions(pick_place_pose_loader
                           PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 nlohmann_json
                          rbs_utils)
rclcpp_components_register_node(
  pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
  EXECUTABLE pick_place_pose_loader_service_server)

# -- MoveitActionServers -- 
add_executable(move_to_joint_states_action_server
               src/move_to_joint_states_action_server.cpp)
ament_target_dependencies(move_to_joint_states_action_server ${deps})

add_executable(move_topose_action_server src/move_topose_action_server.cpp)
ament_target_dependencies(move_topose_action_server ${deps})

add_executable(move_cartesian_path_action_server
               src/move_cartesian_path_action_server.cpp)
ament_target_dependencies(move_cartesian_path_action_server ${deps})

# add_executable(add_planning_scene_object_service
#                src/add_planning_scene_objects_service.cpp)
# ament_target_dependencies(add_planning_scene_object_service ${deps})

# add_library(assemble_state_server SHARED src/assemble_state_server.cpp)
# target_compile_definitions(assemble_state_server
#                            PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL")
# ament_target_dependencies(assemble_state_server ${deps})
# target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
# rclcpp_components_register_node(
#   assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE
#   assemble_state_service_server)

# add_library(moveit_update_planning_scene_server SHARED
#             src/moveit_update_planning_scene.cpp)
# target_compile_definitions(
#   moveit_update_planning_scene_server
#   PRIVATE "MOVEIT_UPDATE_PLANNING_SCENE_SERVER_CPP_BUILDING_DLL")
# ament_target_dependencies(moveit_update_planning_scene_server ${deps})
# target_link_libraries(moveit_update_planning_scene_server ${TINYXML2_LIBRARY})
# rclcpp_components_register_node(
#   moveit_update_planning_scene_server PLUGIN "UpdatePlanningSceneServer"
#   EXECUTABLE moveit_update_planning_scene_service_server)
#
install(DIRECTORY include/ DESTINATION include)

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

install(
  TARGETS move_topose_action_server
          gripper_action_server
          pick_place_pose_loader
          move_to_joint_states_action_server
          move_cartesian_path_action_server
          # add_planning_scene_object_service
          # assemble_state_server
          # moveit_update_planning_scene_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME})

ament_export_include_directories(include)

ament_package()