cmake_minimum_required(VERSION 3.8)
project(rbs_skill_servers)

# Set default standards
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()
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()

set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS TRUE)

# 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(Eigen3 3.3 REQUIRED)
find_package(rbs_utils REQUIRED)
find_package(controller_manager_msgs REQUIRED)
find_package(control_msgs REQUIRED)

set(DEPS
    rclcpp
    rclcpp_action
    moveit_core
    moveit_ros_planning
    moveit_ros_planning_interface
    control_msgs
    controller_manager_msgs
    moveit_msgs
    geometry_msgs
    tf2_ros
    rclcpp_components
    rbs_skill_interfaces
    tf2_eigen
    tf2_msgs
    geometric_shapes
)


macro(add_ros2_component target_name source_file plugin_name executable_name)
  add_library(${target_name} SHARED ${source_file})
  target_include_directories(${target_name}
    PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
  target_compile_definitions(${target_name} PRIVATE "${target_name}_BUILDING_DLL")
  ament_target_dependencies(${target_name} ${DEPS})
  rclcpp_components_register_node(${target_name} PLUGIN ${plugin_name} EXECUTABLE ${executable_name})
endmacro()

# Add components
add_ros2_component(gripper_action_server
    src/gripper_command.cpp
    "rbs_skill_actions::GripperControlActionServer"
    gripper_command)

add_ros2_component(cartesian_move_to_pose
    src/mtp_cart.cpp
    "rbs_skill_actions::CartesianMoveToPose"
    mtp_cart)

add_ros2_component(move_to_joint_states
    src/mtjs_jtc.cpp
    "rbs_skill_actions::MoveToJointStateActionServer"
    mtjs_jtc)

add_ros2_component(move_to_pose
    src/mtp_jtc.cpp
    "rbs_skill_actions::MoveToPose"
    mtp_jtc)

add_ros2_component(move_to_pose_cartesian
    src/mtp_jtc_cart.cpp
    "rbs_skill_actions::MoveToPoseJtcCartesian"
    mtp_jtc_cart)

add_ros2_component(move_to_pose_moveit
    src/mtp_moveit.cpp
    "rbs_skill_actions::MoveitMtp"
    mtp_moveit)

add_ros2_component(move_to_pose_moveit_cartesian
    src/mtp_moveit_cart.cpp
    "rbs_skill_actions::MoveitMtpCart"
    mtp_moveit_cart)

# Install directories and targets
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})

install(
  TARGETS 
          gripper_action_server
          cartesian_move_to_pose
          move_to_joint_states
          move_to_pose
          move_to_pose_cartesian
          move_to_pose_moveit_cartesian
          move_to_pose_moveit
  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()