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(rbs_utils REQUIRED)
find_package(moveit_servo 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
    moveit_servo
    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 
#                           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(servo_action_server
#                src/moveit_servo_skill_server.cpp)
# ament_target_dependencies(servo_action_server ${deps})
#
install(DIRECTORY include/ DESTINATION include)

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

install(
  TARGETS move_topose_action_server
          gripper_action_server
          move_to_joint_states_action_server
          move_cartesian_path_action_server
          # servo_action_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME})

ament_export_include_directories(include)

ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
  scripts/test_cartesian_controller.py
  scripts/move_to_pose.py
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()