CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(aerial_autonomy)

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
set(CMAKE_CXX_FLAGS "-O3 -g -Werror -Wall -std=c++11 --coverage -march=corei7 -ffast-math -Wno-strict-overflow ${CMAKE_CXX_FLAGS}")
add_definitions(-DBOOST_MPL_CFG_NO_PREPROCESSED_HEADERS -DBOOST_MPL_LIMIT_MAP_SIZE=50 -DBOOST_MPL_LIMIT_VECTOR_SIZE=50 -DFUSION_MAX_VECTOR_SIZE=50
                -DPROJECT_SOURCE_DIR="${PROJECT_SOURCE_DIR}")
add_definitions(-std=c++11)

option(USE_ARM_PLUGINS "Use Arm Plugins" ON)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  geometry_msgs
  sensor_msgs
  parsernode
  class_loader
  pluginlib
  rostest
  tf_conversions
  quad_simulator_parser
  tf
  cv_bridge
  image_transport
  ar_track_alvar_msgs
  dynamic_reconfigure
  gcop_comm
)

if(USE_ARM_PLUGINS)
  find_package(arm_plugins)
  if (NOT arm_plugins_FOUND)
    message(WARNING "Could not find arm_plugins.  Not building arm-related components.")
    set(USE_ARM_PLUGINS OFF)
  else ()
    set(catkin_INCLUDE_DIRS ${catkin_INCLUDE_DIRS} ${arm_plugins_INCLUDE_DIRS})
    set(catkin_LIBRARIES ${catkin_LIBRARIES} ${arm_plugins_LIBRARIES})
  endif ()
endif()

include(cmake/add_event_function.cmake)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
find_package(Protobuf 3.0 REQUIRED)
find_package(Glog REQUIRED)
find_package(Armadillo REQUIRED)

#Quad simulator parser dependencies
find_package(GCOP REQUIRED systems)
include_directories(${GCOP_INCLUDE_DIRS})

find_package(TinyXML REQUIRED)
include_directories(${TINYXML_INCLUDE_DIR})
set(GCOP_LIBRARIES ${GCOP_LIBRARIES} ${TINYXML_LIBRARIES})

find_package(casadi)
if (casadi_FOUND)
  message("Found CASADI")
  include_directories(${CASADI_INCLUDE_DIRS})
  link_directories( ${CASADI_LIBRARY_DIRS})
  set(GCOP_LIBRARIES ${GCOP_LIBRARIES} ${CASADI_LIBRARIES})
endif()

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

find_package(OpenCV OPTIONAL_COMPONENTS tracking)

if (OpenCV_VERSION_MAJOR EQUAL "3")
  message(STATUS "OpenCV3 version requires tracking module to be included")
  if (NOT OpenCV_tracking_FOUND)
    message(FATAL_ERROR "Opencv3 Tracking Module not found")
  endif()
endif ()


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
 add_message_files(
   FILES
   VelocityYaw.msg
 )

## Generate services in the 'srv' folder
#add_service_files(
#FILES
#Service1.srv
#Service2.srv
#)

## Generate actions in the 'action' folder
#add_action_files(
#FILES
#Action1.action
#Action2.action
#)

## Generate added messages and services with any dependencies listed here
generate_messages(
#DEPENDENCIES
#std_msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
  cfg/VelocityBasedPositionControllerDynamic.cfg
#cfg / DynReconf2.cfg
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES aerial_autonomy
#CATKIN_DEPENDS roscpp std_msgs
#DEPENDS system_lib
)

######################
## Event Generation ##
######################
## Generates event header files
## ADD ${$ { PROJECT_NAME }_EXPORTED_TARGETS}as a dependency to any
##targets that use the event header files
##This target is also common to messages, cfg files etc
##Add $ {EVENT_INCLUDE_DIR} to include_directories
## To use the event header files in source code, include
## them as #include <${PROJECT_NAME}/event_file_name.h>
## The header file contains the structs to define events
## It also adds an event manager class templated on StateMachine
## to trigger events based on name:
##  void triggerEvent(event_name, state_machine)
## It also lets you print all the event names created:
##  void printEventList()
generate_event_targets(
uav_basic_events
visual_servoing_events
pick_place_events
sensor_place_events
arm_events
uav_arm_sysid_events
mpc_events
)

##############
## Protobuf ##
##############
PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HEADER
  proto/uav_system_config.proto
  proto/uav_vision_system_config.proto
  proto/uav_arm_system_config.proto
  proto/common_system_handler_config.proto
  proto/uav_system_handler_config.proto
  proto/uav_arm_system_handler_config.proto
  proto/velocity_based_position_controller_config.proto
  proto/velocity_based_relative_pose_controller_config.proto
  proto/constant_heading_depth_controller_config.proto
  proto/position_controller_config.proto
  proto/pose_controller_config.proto
  proto/velocity_controller_config.proto
  proto/rpyt_based_velocity_controller_config.proto
  proto/rpyt_based_relative_pose_controller_config.proto
  proto/rpyt_based_position_controller_config.proto
  proto/joystick_velocity_controller_config.proto
  proto/position.proto
  proto/rotation.proto
  proto/position_yaw.proto
  proto/transform.proto
  proto/velocity.proto
  proto/data_stream_config.proto
  proto/log_config.proto
  proto/arm_system_config.proto
  proto/base_state_machine_config.proto
  proto/uav_state_machine_config.proto
  proto/mpc_state_machine_config.proto
  proto/visual_servoing_state_machine_config.proto
  proto/pick_place_state_machine_config.proto
  proto/sensor_place_state_machine_config.proto
  proto/following_waypoint_sequence_config.proto
  proto/grip_config.proto
  proto/place_group.proto
  proto/acceleration_bias_estimator_config.proto
  proto/thrust_gain_estimator_config.proto
  proto/tracking_vector_estimator_config.proto
  proto/ros_sensor_config.proto
  proto/arm_sine_controller_config.proto
  proto/qrotor_backstepping_controller_config.proto
  proto/ddp_mpc_controller_config.proto
  proto/airm_mpc_controller_config.proto
  proto/mpc_visualizer_config.proto
  proto/spiral_reference_trajectory_config.proto
  proto/particle_reference_config.proto
  proto/quad_mpc_controller_config.proto
  proto/mpc_connector_config.proto
  proto/rpyt_reference_connector_config.proto
  proto/polynomial_reference_config.proto
  proto/odom_from_pose_sensor_config.proto
)
add_library(proto ${PROTO_HEADER} ${PROTO_SRC})

target_link_libraries(proto ${PROTOBUF_LIBRARIES})

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${Boost_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
  ${PROTOBUF_INCLUDE_DIR}
  ${CMAKE_CURRENT_BINARY_DIR}
  ${GLOG_INCLUDE_DIR}
)

set(ARM_SRC
  src/controller_connectors/arm_sine_controller_connector.cpp
  src/controller_connectors/builtin_pose_controller_arm_connector.cpp
  src/controller_connectors/visual_servoing_controller_arm_connector.cpp
  src/controller_connectors/mpc_controller_airm_connector.cpp
)

set(SRC
  src/common/async_timer.cpp
  src/common/math.cpp
  src/common/conversions.cpp
  src/common/controller_status.cpp
  src/common/string_utils.cpp
  src/common/system_handler_node_utils.cpp
  src/common/mpc_trajectory_visualizer.cpp
  src/log/data_stream.cpp
  src/log/log.cpp
  src/log/mocap_logger.cpp
  src/trackers/roi_base_tracker.cpp
  src/trackers/roi_to_position_converter.cpp
  src/trackers/roi_to_plane_converter.cpp
  src/trackers/simple_tracker.cpp
  src/trackers/alvar_tracker.cpp
  src/trackers/base_tracker.cpp
  src/trackers/simple_multi_tracker.cpp
  src/trackers/simulated_ros_tracker.cpp
  src/trackers/closest_tracking_strategy.cpp
  src/trackers/id_tracking_strategy.cpp
  src/trackers/simple_tracking_strategy.cpp
  src/types/spiral_reference_trajectory.cpp
  src/types/quad_particle_reference_trajectory.cpp
  src/types/polynomial_reference_trajectory.cpp
  src/types/sensor_status.cpp
  src/controllers/manual_rpyt_controller.cpp
  src/controllers/velocity_based_position_controller.cpp
  src/controllers/constant_heading_depth_controller.cpp
  src/controllers/relative_pose_controller.cpp
  src/controllers/velocity_based_relative_pose_controller.cpp
  src/controllers/rpyt_based_velocity_controller.cpp
  src/controllers/rpyt_based_position_controller.cpp
  src/controllers/rpyt_based_relative_pose_controller.cpp
  src/controllers/joystick_velocity_controller.cpp
  src/controllers/arm_sine_controller.cpp
  src/controllers/qrotor_backstepping_controller.cpp
  src/controllers/ddp_casadi_mpc_controller.cpp
  src/controllers/ddp_quad_mpc_controller.cpp
  src/controllers/quad_particle_reference_controller.cpp
  src/controllers/quad_polynomial_reference_controller.cpp
  src/controllers/rpyt_based_reference_controller.cpp
  src/estimators/acceleration_bias_estimator.cpp
  src/estimators/thrust_gain_estimator.cpp
  src/estimators/tracking_vector_estimator.cpp
  src/controller_connectors/visual_servoing_controller_drone_connector.cpp
  src/controller_connectors/base_relative_pose_visual_servoing_connector.cpp
  src/controller_connectors/relative_pose_visual_servoing_controller_drone_connector.cpp
  src/controller_connectors/rpyt_relative_pose_visual_servoing_connector.cpp
  src/controller_connectors/position_controller_drone_connector.cpp
  src/controller_connectors/velocity_based_position_controller_drone_connector.cpp
  src/controller_connectors/rpyt_based_position_controller_drone_connector.cpp
  src/controller_connectors/rpyt_based_reference_connector.cpp
  src/controller_connectors/builtin_velocity_controller_drone_connector.cpp
  src/controller_connectors/manual_rpyt_controller_drone_connector.cpp
  src/controller_connectors/joystick_velocity_controller_drone_connector.cpp
  src/controller_connectors/base_mpc_controller_quad_connector.cpp
  src/controller_connectors/mpc_controller_quad_connector.cpp
  src/controller_connectors/qrotor_backstepping_controller_connector.cpp
  src/sensors/pose_sensor.cpp
  src/sensors/odometry_from_pose_sensor.cpp
)

if (casadi_FOUND)
  set(ARM_SRC ${ARM_SRC}
      src/controllers/ddp_airm_mpc_controller.cpp)
endif()

if (USE_ARM_PLUGINS)
  set(SRC ${SRC} ${ARM_SRC})
endif ()


## Declare a C++ library
add_library(aerial_autonomy
  ${SRC}
)
add_dependencies(aerial_autonomy proto)
target_link_libraries(aerial_autonomy proto ${GCOP_LIBRARIES} ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS} ${ARMADILLO_LIBRARY})


## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(aerial_autonomy ${${PROJECT_NAME}_EXPORTED_TARGETS})

## Declare a C++ executable
add_executable(joystick_uav_node src/system_handler_nodes/joystick_uav_node.cpp)
add_executable(uav_system_node src/system_handler_nodes/uav_system_node.cpp)
add_executable(uav_vision_system_node src/system_handler_nodes/uav_vision_system_node.cpp)
add_executable(event_publish_node src/tests/event_publish_node.cpp)
add_dependencies(event_publish_node ${PROJECT_NAME}_generate_messages_cpp)
add_executable(qrotor_backstepping_controller_tuner src/controller_tuners/qrotor_backstepping_controller_tuner.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
#add_dependencies(aerial_autonomy_node ${${PROJECT_NAME} _EXPORTED_TARGETS } ${catkin_EXPORTED_TARGETS })

## Specify libraries to link a library or executable target against

target_link_libraries(joystick_uav_node aerial_autonomy)
target_link_libraries(uav_system_node aerial_autonomy)
target_link_libraries(uav_vision_system_node aerial_autonomy)
target_link_libraries(event_publish_node ${catkin_LIBRARIES})
target_link_libraries(qrotor_backstepping_controller_tuner aerial_autonomy)

if (USE_ARM_PLUGINS)
  add_executable(airm_mpc_node src/system_handler_nodes/airm_mpc_node.cpp)
  add_executable(uav_arm_system_node src/system_handler_nodes/uav_arm_system_node.cpp)
  add_executable(sensor_place_node src/system_handler_nodes/sensor_place_node.cpp)
  add_executable(uav_arm_sysid_node src/system_handler_nodes/uav_arm_sysid_node.cpp)

  target_link_libraries(uav_arm_system_node aerial_autonomy)
  target_link_libraries(sensor_place_node aerial_autonomy)
  target_link_libraries(uav_arm_sysid_node aerial_autonomy)
  target_link_libraries(airm_mpc_node aerial_autonomy)
  if (casadi_FOUND)
    add_executable(mpc_controller_tuner src/controller_tuners/mpc_controller_tuner.cpp)
    add_executable(quad_mpc_controller_tuner src/controller_tuners/quad_mpc_controller_tuner.cpp)
    target_link_libraries(mpc_controller_tuner aerial_autonomy)
    target_link_libraries(quad_mpc_controller_tuner aerial_autonomy)
  endif()
endif ()

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
catkin_add_gtest(${PROJECT_NAME}-logic-states-test tests/logic_states/base_state_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-timed-state-test tests/logic_states/timed_state_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-ctrlr-hrdwr-cnctr-test tests/controller_connectors/controller_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-evnt-mngr-test tests/event_manager_tests/event_manager_tests.cpp)
add_dependencies(${PROJECT_NAME}-evnt-mngr-test ${${PROJECT_NAME}_EXPORTED_TARGETS})
catkin_add_gtest(${PROJECT_NAME}-type-map-test tests/common/type_map_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-unordered-heterogeneous-map-test tests/common/unordered_heterogeneous_map_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-html-utils-test tests/common/html_utils_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-pos-ctrlr-drone-cnctr-test tests/controller_connectors/position_controller_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-builtin-vel-ctrlr-drone-cnctr-test tests/controller_connectors/builtin_velocity_controller_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-manual-rpyt-ctrlr-drone-cnctr-test tests/controller_connectors/manual_rpyt_controller_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-uav-system-test tests/robot_systems/uav_system_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-uav-basic-functor-tests tests/actions_guards/uav_basic_functor_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-shorting-action-sequence-tests tests/actions_guards/shorting_action_sequence_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-visual-servoing-functor-tests tests/actions_guards/visual_servoing_functor_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-uav-basic-state-machine-test tests/state_machines/uav_state_machine_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-visual-servoing-state-machine-test tests/state_machines/visual_servoing_state_machine_tests.cpp)
add_dependencies(${PROJECT_NAME}-visual-servoing-state-machine-test ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_rostest_gtest(${PROJECT_NAME}-system-handler-node-utils-test tests/common/system_handler_node_utils_tests.test tests/common/system_handler_node_utils_tests.cpp)

add_rostest_gtest(${PROJECT_NAME}-uav-vision-system-test tests/robot_systems/uav_vision_system_tests.test tests/robot_systems/uav_vision_system_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-state-machine-gui-connector-event-test tests/state_machines/state_machine_gui_connector_event_tests.test tests/state_machines/state_machine_gui_connector_event_tests.cpp)
add_dependencies(${PROJECT_NAME}-state-machine-gui-connector-event-test ${${PROJECT_NAME}_EXPORTED_TARGETS} event_publish_node)
add_rostest_gtest(${PROJECT_NAME}-state-machine-gui-connector-pose-test tests/state_machines/state_machine_gui_connector_pose_tests.test tests/state_machines/state_machine_gui_connector_pose_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-state-machine-gui-connector-velocity-test tests/state_machines/state_machine_gui_connector_velocity_tests.test tests/state_machines/state_machine_gui_connector_velocity_tests.cpp)
add_dependencies(${PROJECT_NAME}-state-machine-gui-connector-pose-test ${${PROJECT_NAME}_EXPORTED_TARGETS} event_publish_node)
add_dependencies(${PROJECT_NAME}-state-machine-gui-connector-velocity-test ${${PROJECT_NAME}_EXPORTED_TARGETS} event_publish_node)
catkin_add_gtest(${PROJECT_NAME}-exponential-filter-test tests/filters/exponential_filter_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-async-timer-test tests/common/async_timer_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-atomic-test tests/common/atomic_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-controller-status-test tests/common/controller_status_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-uav-system-handler-test tests/system_handlers/uav_system_handler_tests.test tests/system_handlers/uav_system_handler_tests.cpp)
add_dependencies(${PROJECT_NAME}-uav-system-handler-test ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_rostest_gtest(${PROJECT_NAME}-uav-vision-system-handler-test tests/system_handlers/uav_vision_system_handler_tests.test tests/system_handlers/uav_vision_system_handler_tests.cpp)
add_dependencies(${PROJECT_NAME}-uav-vision-system-handler-test ${${PROJECT_NAME}_EXPORTED_TARGETS})
catkin_add_gtest(${PROJECT_NAME}-thread-safe-state-machine-test tests/common/thread_safe_state_machine_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-iterable-enum-test tests/common/iterable_enum_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-velocity-based-position-controller-test tests/controllers/velocity_based_position_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-arm-sine-controller-test tests/controllers/arm_sine_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-thrust-gain-estimator-test tests/estimators/thrust_gain_estimator_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-acceleration-bias-estimator-test tests/estimators/acceleration_bias_estimator_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-tracking-vector-estimator-test tests/estimators/tracking_vector_estimator_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-relative-pose-controller-test tests/controllers/relative_pose_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-velocity-based-relative-pose-controller-test tests/controllers/velocity_based_relative_pose_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-relative-pose-controller-test tests/controllers/rpyt_based_relative_pose_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-velocity-controller-test tests/controllers/rpyt_based_velocity_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-position-controller-test tests/controllers/rpyt_based_position_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-reference-controller-test tests/controllers/rpyt_based_reference_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-constant-heading-depth-controller-test tests/controllers/constant_heading_depth_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-math-test tests/common/math_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-simple-tracker-test tests/trackers/simple_tracker_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-simple-multi-tracker-test tests/trackers/simple_multi_tracker_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-roi-to-position-converter-test tests/trackers/roi_to_position_converter_tests.test tests/trackers/roi_to_position_converter_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-roi-to-plane-converter-test tests/trackers/roi_to_plane_converter_tests.test tests/trackers/roi_to_plane_converter_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-alvar-tracker-test tests/trackers/alvar_tracker_tests.test tests/trackers/alvar_tracker_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-velocity-sensor-test tests/sensors/velocity_sensor_tests.test tests/sensors/velocity_sensor_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-relative-pose-visual-servoing-drone-connector-test tests/controller_connectors/relative_pose_visual_servoing_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-relative-pose-visual-servoing-connector-test tests/controller_connectors/rpyt_relative_pose_visual_servoing_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-visual-servoing-reference-connector-test tests/controller_connectors/visual_servoing_reference_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-position-controller-drone-connector-test tests/controller_connectors/rpyt_based_position_controller_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-rpyt-based-reference-connector-test tests/controller_connectors/rpyt_based_reference_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-ddp-quad-mpc-connector-test tests/controller_connectors/ddp_quad_mpc_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-visual-servoing-drone-connector-test tests/controller_connectors/visual_servoing_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-joystick-velocity-controller-drone-connector-test tests/controller_connectors/joystick_velocity_controller_drone_connector_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-data-stream-test tests/log/data_stream_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-log-test tests/log/log_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-string-utils-test tests/common/string_utils_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-conversions-test tests/common/conversions_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-proto-utils-test tests/common/proto_utils_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-reference-trajectory-test tests/types/reference_trajectory_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-airm-spiral-reference-trajectory-test tests/types/airm_spiral_reference_trajectory_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-quad-particle-reference-trajectory-test tests/types/quad_particle_reference_trajectory_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-polynomial-reference-trajectory-test tests/types/polynomial_reference_trajectory_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-qrotor-backstepping-controller-test tests/controllers/qrotor_backstepping_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-ddp-quad-mpc-controller-test tests/controllers/ddp_quad_mpc_controller_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-minimum-snap-trajectory-test tests/types/minimum_snap_trajectory_tests.cpp)
catkin_add_gtest(${PROJECT_NAME}-qrotor-backstepping-controller-connector-test tests/controller_connectors/qrotor_backstepping_controller_connector_tests.cpp)
add_rostest_gtest(${PROJECT_NAME}-qrotor-backstepping-trajectory-visualizer-test tests/common/qrotor_backstepping_trajectory_visualizer_tests.test tests/common/qrotor_backstepping_trajectory_visualizer_tests.cpp)
if(TARGET ${PROJECT_NAME}-uav-basic-state-machine-test)
  target_link_libraries(${PROJECT_NAME}-uav-basic-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-visual-servoing-state-machine-test)
  target_link_libraries(${PROJECT_NAME}-visual-servoing-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-pos-ctrlr-drone-cnctr-test)
  target_link_libraries(${PROJECT_NAME}-pos-ctrlr-drone-cnctr-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-builtin-vel-ctrlr-drone-cnctr-test)
  target_link_libraries(${PROJECT_NAME}-builtin-vel-ctrlr-drone-cnctr-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-manual-rpyt-ctrlr-drone-cnctr-test)
  target_link_libraries(${PROJECT_NAME}-manual-rpyt-ctrlr-drone-cnctr-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-joystick-velocity-controller-drone-connector-test)
  target_link_libraries(${PROJECT_NAME}-joystick-velocity-controller-drone-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-unordered-heterogeneous-map-test)
  target_link_libraries(${PROJECT_NAME}-unordered-heterogeneous-map-test ${GLOG_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-logic-states-test)
  target_link_libraries(${PROJECT_NAME}-logic-states-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-timed-state-test)
  target_link_libraries(${PROJECT_NAME}-timed-state-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-evnt-mngr-test)
  target_link_libraries(${PROJECT_NAME}-evnt-mngr-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-uav-system-test)
  target_link_libraries(${PROJECT_NAME}-uav-system-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-uav-basic-functor-tests)
  target_link_libraries(${PROJECT_NAME}-uav-basic-functor-tests aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-shorting-action-sequence-tests)
  target_link_libraries(${PROJECT_NAME}-shorting-action-sequence-tests aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-visual-servoing-functor-tests)
  target_link_libraries(${PROJECT_NAME}-visual-servoing-functor-tests aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-ctrlr-hrdwr-cnctr-test)
  target_link_libraries(${PROJECT_NAME}-ctrlr-hrdwr-cnctr-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-state-machine-gui-connector-event-test)
  target_link_libraries(${PROJECT_NAME}-state-machine-gui-connector-event-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-state-machine-gui-connector-pose-test)
  target_link_libraries(${PROJECT_NAME}-state-machine-gui-connector-pose-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-state-machine-gui-connector-velocity-test)
  target_link_libraries(${PROJECT_NAME}-state-machine-gui-connector-velocity-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-async-timer-test)
  target_link_libraries(${PROJECT_NAME}-async-timer-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-exponential-filter-test)
  target_link_libraries(${PROJECT_NAME}-exponential-filter-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-atomic-test)
  target_link_libraries(${PROJECT_NAME}-atomic-test ${Boost_LIBRARIES} ${catkin_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-controller-status-test)
  target_link_libraries(${PROJECT_NAME}-controller-status-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-thread-safe-state-machine-test)
  target_link_libraries(${PROJECT_NAME}-thread-safe-state-machine-test ${Boost_LIBRARIES} ${catkin_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-uav-system-handler-test)
  target_link_libraries(${PROJECT_NAME}-uav-system-handler-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-uav-vision-system-handler-test)
  target_link_libraries(${PROJECT_NAME}-uav-vision-system-handler-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-uav-vision-system-test)
  target_link_libraries(${PROJECT_NAME}-uav-vision-system-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-velocity-based-position-controller-test)
  target_link_libraries(${PROJECT_NAME}-velocity-based-position-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-arm-sine-controller-test)
  target_link_libraries(${PROJECT_NAME}-arm-sine-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-thrust-gain-estimator-test)
  target_link_libraries(${PROJECT_NAME}-thrust-gain-estimator-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-acceleration-bias-estimator-test)
  target_link_libraries(${PROJECT_NAME}-acceleration-bias-estimator-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-tracking-vector-estimator-test)
  target_link_libraries(${PROJECT_NAME}-tracking-vector-estimator-test aerial_autonomy ${catkin_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-relative-pose-controller-test)
  target_link_libraries(${PROJECT_NAME}-relative-pose-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-velocity-based-relative-pose-controller-test)
  target_link_libraries(${PROJECT_NAME}-velocity-based-relative-pose-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-relative-pose-controller-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-relative-pose-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-position-controller-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-position-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-reference-controller-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-reference-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-position-controller-drone-connector-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-position-controller-drone-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-reference-connector-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-reference-connector-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-ddp-quad-mpc-connector-test)
  target_link_libraries(${PROJECT_NAME}-ddp-quad-mpc-connector-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-rpyt-based-velocity-controller-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-based-velocity-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-constant-heading-depth-controller-test)
  target_link_libraries(${PROJECT_NAME}-constant-heading-depth-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-qrotor-backstepping-controller-test)
  target_link_libraries(${PROJECT_NAME}-qrotor-backstepping-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-ddp-quad-mpc-controller-test)
  target_link_libraries(${PROJECT_NAME}-ddp-quad-mpc-controller-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-relative-pose-visual-servoing-drone-connector-test)
  target_link_libraries(${PROJECT_NAME}-relative-pose-visual-servoing-drone-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-rpyt-relative-pose-visual-servoing-connector-test)
  target_link_libraries(${PROJECT_NAME}-rpyt-relative-pose-visual-servoing-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-visual-servoing-reference-connector-test)
  target_link_libraries(${PROJECT_NAME}-visual-servoing-reference-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-visual-servoing-drone-connector-test)
  target_link_libraries(${PROJECT_NAME}-visual-servoing-drone-connector-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-roi-to-position-converter-test)
  target_link_libraries(${PROJECT_NAME}-roi-to-position-converter-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-roi-to-plane-converter-test)
  target_link_libraries(${PROJECT_NAME}-roi-to-plane-converter-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-system-handler-node-utils-test)
  target_link_libraries(${PROJECT_NAME}-system-handler-node-utils-test aerial_autonomy)
endif()

if(TARGET ${PROJECT_NAME}-alvar-tracker-test)
  target_link_libraries(${PROJECT_NAME}-alvar-tracker-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-simple-tracker-test)
  target_link_libraries(${PROJECT_NAME}-simple-tracker-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-simple-multi-tracker-test)
  target_link_libraries(${PROJECT_NAME}-simple-multi-tracker-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-math-test)
  target_link_libraries(${PROJECT_NAME}-math-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-data-stream-test)
  target_link_libraries(${PROJECT_NAME}-data-stream-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-log-test)
  target_link_libraries(${PROJECT_NAME}-log-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-string-utils-test)
  target_link_libraries(${PROJECT_NAME}-string-utils-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-proto-utils-test)
  target_link_libraries(${PROJECT_NAME}-proto-utils-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-conversions-test)
  target_link_libraries(${PROJECT_NAME}-conversions-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-velocity-sensor-test)
  target_link_libraries(${PROJECT_NAME}-velocity-sensor-test aerial_autonomy ${GCOP_LIBRARIES})
endif()
if(TARGET ${PROJECT_NAME}-reference-trajectory-test)
  target_link_libraries(${PROJECT_NAME}-reference-trajectory-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-airm-spiral-reference-trajectory-test)
  target_link_libraries(${PROJECT_NAME}-airm-spiral-reference-trajectory-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-quad-particle-reference-trajectory-test)
  target_link_libraries(${PROJECT_NAME}-quad-particle-reference-trajectory-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-polynomial-reference-trajectory-test)
  target_link_libraries(${PROJECT_NAME}-polynomial-reference-trajectory-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-minimum-snap-trajectory-test)
  target_link_libraries(${PROJECT_NAME}-minimum-snap-trajectory-test aerial_autonomy)
endif()
if(TARGET ${PROJECT_NAME}-qrotor-backstepping-controller-connector-test)
  target_link_libraries(${PROJECT_NAME}-qrotor-backstepping-controller-connector-test aerial_autonomy ${GCOP_LIBRARIES} ${TINYXML_LIBRARIES} ${QUAD_SIM_PARSER_LIBS})
endif()
if(TARGET ${PROJECT_NAME}-qrotor-backstepping-trajectory-visualizer-test)
  target_link_libraries(${PROJECT_NAME}-qrotor-backstepping-trajectory-visualizer-test aerial_autonomy ${GCOP_LIBRARIES} ${TINYXML_LIBRARIES} ${QUAD_SIM_PARSER_LIBS})
endif()
if (USE_ARM_PLUGINS)
  catkin_add_gtest(${PROJECT_NAME}-joystick-state-machine-test tests/state_machines/joystick_state_machine_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-pick-place-state-machine-test tests/state_machines/pick_place_state_machine_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-sensor-place-state-machine-test tests/state_machines/sensor_place_state_machine_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-mpc-state-machine-test tests/state_machines/mpc_state_machine_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-sensor-place-functor-tests tests/actions_guards/sensor_place_functor_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-mpc-functor-tests tests/actions_guards/mpc_functor_tests.cpp)
  add_rostest_gtest(${PROJECT_NAME}-uav-arm-system-handler-test tests/system_handlers/uav_arm_system_handler_tests.test tests/system_handlers/uav_arm_system_handler_tests.cpp)
  add_dependencies(${PROJECT_NAME}-uav-arm-system-handler-test ${${PROJECT_NAME}_EXPORTED_TARGETS})
  add_rostest_gtest(${PROJECT_NAME}-uav-arm-system-test tests/robot_systems/uav_arm_system_tests.test tests/robot_systems/uav_arm_system_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-visual-servoing-arm-connector-test tests/controller_connectors/visual_servoing_arm_connector_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-arm-functor-tests tests/actions_guards/arm_functor_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-uav-arm-sysid-state-machine-test tests/state_machines/uav_arm_sysid_state_machine_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-arm-system-test tests/robot_systems/arm_system_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-ddp-airm-mpc-connector-test tests/controller_connectors/ddp_airm_mpc_connector_tests.cpp)
  catkin_add_gtest(${PROJECT_NAME}-ddp-airm-mpc-controller-test tests/controllers/ddp_airm_mpc_controller_tests.cpp)
  add_rostest_gtest(${PROJECT_NAME}-mpc-trajectory-visualizer-test tests/common/mpc_trajectory_visualizer_tests.test tests/common/mpc_trajectory_visualizer_tests.cpp)

  if(TARGET ${PROJECT_NAME}-joystick-state-machine-test)
    target_link_libraries(${PROJECT_NAME}-joystick-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-arm-functor-tests)
    target_link_libraries(${PROJECT_NAME}-arm-functor-tests aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-uav-arm-system-handler-test)
    target_link_libraries(${PROJECT_NAME}-uav-arm-system-handler-test aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-uav-arm-sysid-state-machine-test)
    target_link_libraries(${PROJECT_NAME}-uav-arm-sysid-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-uav-arm-system-test)
    target_link_libraries(${PROJECT_NAME}-uav-arm-system-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-arm-system-test)
    target_link_libraries(${PROJECT_NAME}-arm-system-test aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-visual-servoing-arm-connector-test)
    target_link_libraries(${PROJECT_NAME}-visual-servoing-arm-connector-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-pick-place-state-machine-test)
    target_link_libraries(${PROJECT_NAME}-pick-place-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-sensor-place-state-machine-test)
    target_link_libraries(${PROJECT_NAME}-sensor-place-state-machine-test aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-mpc-state-machine-test)
    target_link_libraries(${PROJECT_NAME}-mpc-state-machine-test aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-pick-place-functor-tests)
    target_link_libraries(${PROJECT_NAME}-pick-place-functor-tests aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-sensor-place-functor-tests)
    target_link_libraries(${PROJECT_NAME}-sensor-place-functor-tests aerial_autonomy ${GCOP_LIBRARIES})
  endif()
  if(TARGET ${PROJECT_NAME}-mpc-functor-tests)
    target_link_libraries(${PROJECT_NAME}-mpc-functor-tests aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-ddp-airm-mpc-controller-test)
    target_link_libraries(${PROJECT_NAME}-ddp-airm-mpc-controller-test aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-ddp-airm-mpc-connector-test)
    target_link_libraries(${PROJECT_NAME}-ddp-airm-mpc-connector-test aerial_autonomy)
  endif()
  if(TARGET ${PROJECT_NAME}-mpc-trajectory-visualizer-test)
    target_link_libraries(${PROJECT_NAME}-mpc-trajectory-visualizer-test aerial_autonomy)
  endif()

endif ()

## Add folders to be run by python nosetests
#catkin_add_nosetests(test)
install(FILES plugin.xml
        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
        )

## Configure script to generate test coverage
configure_file(scripts/generate_test_coverage.bash.in ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_test_coverage.bash)
## Configure script to generate dot files for state machines
configure_file(scripts/generate_dot_files.py.in ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_dot_files.py)
configure_file(param/log_config.pbtxt.in ${CMAKE_CURRENT_SOURCE_DIR}/param/log_config.pbtxt)