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)

# 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()

add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)

target_include_directories(gripper_action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

  target_include_directories(pick_place_pose_loader PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

target_compile_definitions(gripper_action_server
    PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")

target_compile_definitions(pick_place_pose_loader
    PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")

ament_target_dependencies(gripper_action_server ${deps})
ament_target_dependencies(pick_place_pose_loader ${deps})

rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)

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(pick_place_pose_loader src/get_grasp_pick_pose.cpp)
#ament_target_dependencies(pick_place_pose_loader ${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(
  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()