├── .gitmodules ├── test ├── estimation_manager │ ├── CMakeLists.txt │ └── estimator_switching │ │ ├── config │ │ ├── mrs_simulator.yaml │ │ ├── world_config.yaml │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ └── estimator_switching.test ├── uav_manager │ ├── takeoff_fail_cause_tracker │ │ ├── config │ │ │ ├── automatic_start.yaml │ │ │ ├── mrs_simulator.yaml │ │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ ├── test.cpp │ │ └── takeoff_fail_cause_tracker.test │ ├── takeoff_fail_cause_controller │ │ ├── config │ │ │ ├── automatic_start.yaml │ │ │ ├── mrs_simulator.yaml │ │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ ├── test.cpp │ │ └── takeoff_fail_cause_controller.test │ ├── land_home_moving_takeoff_frame │ │ ├── config │ │ │ ├── automatic_start.yaml │ │ │ ├── mrs_simulator.yaml │ │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ └── land_home_moving_takeoff_frame.test │ ├── midair_fail_cause_wrong_tracker │ │ ├── config │ │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ ├── midair_fail_cause_wrong_tracker.test │ │ └── test.cpp │ ├── midair_fail_cause_wrong_controller │ │ ├── config │ │ │ └── custom_config.yaml │ │ ├── CMakeLists.txt │ │ ├── midair_fail_cause_wrong_controller.test │ │ └── test.cpp │ ├── land_home │ │ ├── config │ │ │ └── mrs_simulator.yaml │ │ ├── CMakeLists.txt │ │ └── land_home.test │ ├── land_there │ │ ├── config │ │ │ ├── mrs_simulator.yaml │ │ │ └── world_config.yaml │ │ ├── CMakeLists.txt │ │ └── land_there.test │ ├── takeoff │ │ ├── config │ │ │ └── mrs_simulator.yaml │ │ ├── CMakeLists.txt │ │ ├── test.cpp │ │ └── takeoff.test │ ├── landing │ │ ├── CMakeLists.txt │ │ └── landing.test │ ├── max_height_check │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── world_config.yaml │ │ │ └── custom_config.yaml │ │ ├── max_height_check.test │ │ └── test.cpp │ ├── max_throttle_check │ │ ├── CMakeLists.txt │ │ └── max_throttle_check.test │ ├── min_height_check │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── world_config.yaml │ │ │ └── custom_config.yaml │ │ └── min_height_check.test │ ├── midair_fail_cause_enabling_output │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── world_config.yaml │ │ ├── test.cpp │ │ └── midair_fail_cause_enabling_output.test │ ├── midair_fail_cause_enabling_offboard │ │ ├── CMakeLists.txt │ │ ├── test.cpp │ │ └── midair_fail_cause_enabling_offboard.test │ └── CMakeLists.txt ├── gain_manager │ ├── CMakeLists.txt │ ├── set_gains_should_fail │ │ ├── CMakeLists.txt │ │ ├── set_gains_should_fail.test │ │ └── test.cpp │ └── set_gains_should_succeed │ │ ├── CMakeLists.txt │ │ ├── set_gains_should_succeed.test │ │ └── test.cpp ├── constraint_manager │ ├── CMakeLists.txt │ ├── constraints_override │ │ ├── CMakeLists.txt │ │ └── constraints_override.test │ ├── set_constraints_should_fail │ │ ├── CMakeLists.txt │ │ ├── set_constraints_should_fail.test │ │ └── test.cpp │ └── set_constraints_should_succeed │ │ ├── CMakeLists.txt │ │ ├── set_constraints_should_succeed.test │ │ └── test.cpp ├── run_tests.sh ├── control_manager │ ├── set_heading_relative_service │ │ ├── config │ │ │ └── mrs_simulator.yaml │ │ ├── CMakeLists.txt │ │ ├── set_heading_relative_service.test │ │ └── test.cpp │ ├── rc_remote_control │ │ ├── config │ │ │ ├── custom_config.yaml │ │ │ └── world_config.yaml │ │ ├── CMakeLists.txt │ │ └── rc_remote_control.test │ ├── trajectory_start_stop_resume │ │ ├── config │ │ │ ├── custom_config.yaml │ │ │ └── world_config.yaml │ │ ├── CMakeLists.txt │ │ └── trajectory_start_stop_resume.test │ ├── bumper │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── world_config.yaml │ │ └── bumper.test │ ├── eland_service │ │ ├── CMakeLists.txt │ │ └── eland_service.test │ ├── goto_service │ │ ├── CMakeLists.txt │ │ ├── goto_service.test │ │ └── test.cpp │ ├── hover_service │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── world_config.yaml │ │ └── hover_service.test │ ├── eland_control_error │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── custom_config.yaml │ │ ├── eland_control_error.test │ │ └── test.cpp │ ├── eland_innovation │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── custom_config.yaml │ │ │ └── world_config.yaml │ │ └── eland_innovation.test │ ├── goto_fcu_service │ │ ├── CMakeLists.txt │ │ ├── goto_fcu_service.test │ │ └── test.cpp │ ├── reference_service │ │ ├── CMakeLists.txt │ │ ├── reference_service.test │ │ └── test.cpp │ ├── reference_topic │ │ ├── CMakeLists.txt │ │ ├── reference_topic.test │ │ └── test.cpp │ ├── set_heading_service │ │ ├── CMakeLists.txt │ │ └── set_heading_service.test │ ├── trajectory_topic │ │ ├── CMakeLists.txt │ │ └── trajectory_topic.test │ ├── bumper_with_rc_remote │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── custom_config.yaml │ │ │ └── world_config.yaml │ │ └── bumper_with_rc_remote.test │ ├── escalating_failsafe_rc │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── world_config.yaml │ │ │ └── custom_config.yaml │ │ ├── escalating_failsafe_rc.test │ │ └── test.cpp │ ├── failsafe_control_error │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── custom_config.yaml │ │ ├── failsafe_control_error.test │ │ └── test.cpp │ ├── goto_altitude_service │ │ ├── CMakeLists.txt │ │ ├── goto_altitude_service.test │ │ └── test.cpp │ ├── goto_relative_service │ │ ├── CMakeLists.txt │ │ ├── goto_relative_service.test │ │ └── test.cpp │ ├── transform_pose_service │ │ ├── CMakeLists.txt │ │ └── transform_pose_service.test │ ├── velocity_reference_topic │ │ ├── CMakeLists.txt │ │ └── velocity_reference_topic.test │ ├── escalating_failsafe_service │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── world_config.yaml │ │ │ └── custom_config.yaml │ │ ├── test.cpp │ │ └── escalating_failsafe_service.test │ ├── transform_reference_service │ │ ├── CMakeLists.txt │ │ └── transform_reference_service.test │ ├── transform_vector3_service │ │ ├── CMakeLists.txt │ │ └── transform_vector3_service.test │ ├── validate_reference_service │ │ ├── CMakeLists.txt │ │ └── validate_reference_service.test │ ├── reference_service_local_origin │ │ ├── CMakeLists.txt │ │ ├── reference_service_local_origin.test │ │ └── test.cpp │ ├── validate_reference_array_service │ │ ├── CMakeLists.txt │ │ └── validate_reference_array_service.test │ └── CMakeLists.txt ├── CMakeLists.txt ├── compile_tests.sh └── gather_coverage.sh ├── README.md ├── config ├── private │ ├── transform_manager │ │ └── transform_manager.yaml │ ├── gain_manager │ │ └── gain_manager.yaml │ ├── constraint_manager │ │ └── constraint_manager.yaml │ ├── uav_manager.yaml │ ├── estimation_manager │ │ └── estimation_manager.yaml │ ├── estimators.yaml │ ├── trackers.yaml │ └── control_manager.yaml ├── debug_verbosity.yaml └── public │ ├── estimation_manager │ └── estimation_manager.yaml │ ├── active_estimators.yaml │ ├── gain_manager │ ├── gain_manager.yaml │ └── gains.yaml │ ├── constraint_manager │ └── constraint_manager.yaml │ ├── controllers.yaml │ └── uav_manager.yaml ├── .gitignore ├── plugins.xml ├── .github └── workflows │ ├── ros_package_build.yml │ └── ros_build_test.yml ├── include ├── mrs_uav_managers │ ├── estimation_manager │ │ ├── private_handlers.h │ │ ├── types.h │ │ └── common_handlers.h │ ├── control_manager │ │ └── private_handlers.h │ └── agl_estimator.h └── control_manager │ └── output_publisher.h ├── package.xml ├── scripts └── get_public_params.py ├── LICENSE └── nodelets.xml /.gitmodules: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /test/estimation_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(estimator_switching) 2 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | handle_takeoff: false 2 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | handle_takeoff: false 2 | -------------------------------------------------------------------------------- /test/gain_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(set_gains_should_succeed) 2 | 3 | add_subdirectory(set_gains_should_fail) 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MRS UAV Managers 2 | 3 | Please follow to the [documentation page](https://ctu-mrs.github.io/docs/features/managers/). 4 | -------------------------------------------------------------------------------- /config/private/transform_manager/transform_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | transform_manager: 4 | 5 | scope_timer: 6 | enabled: false 7 | -------------------------------------------------------------------------------- /test/constraint_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(set_constraints_should_succeed) 2 | 3 | add_subdirectory(set_constraints_should_fail) 4 | 5 | add_subdirectory(constraints_override) 6 | -------------------------------------------------------------------------------- /config/debug_verbosity.yaml: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.mrs_uav_managers=DEBUG 2 | log4j.logger.ros.mrs_uav_trackers=DEBUG 3 | log4j.logger.ros.mrs_uav_controllers=DEBUG 4 | log4j.logger.ros.mrs_uav_state_estimators=DEBUG 5 | -------------------------------------------------------------------------------- /test/uav_manager/land_home_moving_takeoff_frame/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | preflight_check: 2 | 3 | # takeoff not allowed if UAV's height sensor exceeds 4 | height_check: 5 | 6 | enabled: false 7 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_tracker/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | uav_manager: 4 | 5 | midair_activation: 6 | 7 | after_activation: 8 | tracker: "WrongTracker" 9 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_controller/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | uav_manager: 4 | 5 | midair_activation: 6 | 7 | after_activation: 8 | controller: "WrongController" 9 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # ignore temp files 3 | *~ 4 | 5 | # ignore vim temp files 6 | *.swp 7 | *.swo 8 | *.swn 9 | 10 | # Runtime-python 11 | *.pyc 12 | 13 | act.sh 14 | generate.sh 15 | 16 | debian 17 | .obj-x86_64-linux-gnu 18 | -------------------------------------------------------------------------------- /test/run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG 6 | trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR 7 | 8 | catkin test --this -i -s 9 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_relative_service/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | individual_takeoff_platform: 2 | enabled: true 3 | 4 | uav1: 5 | type: "x500" 6 | spawn: 7 | x: 10.0 8 | y: 20.0 9 | z: 2.0 10 | heading: 4.0 11 | -------------------------------------------------------------------------------- /test/uav_manager/land_home/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /test/uav_manager/land_there/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is Null tracker. 4 | 5 | 6 | -------------------------------------------------------------------------------- /test/estimation_manager/estimator_switching/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /test/uav_manager/land_home_moving_takeoff_frame/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: true 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 3.0 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/config/mrs_simulator.yaml: -------------------------------------------------------------------------------- 1 | # turning off for the takeoff test 2 | individual_takeoff_platform: 3 | enabled: false 4 | 5 | uav1: 6 | type: "x500" 7 | spawn: 8 | x: 10.0 9 | y: 20.0 10 | z: 0.5 11 | heading: 1.2 12 | -------------------------------------------------------------------------------- /config/private/gain_manager/gain_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | gain_manager: 4 | 5 | rate: 10.0 # [Hz] 6 | diagnostics_rate: 1.0 # [Hz] 7 | 8 | scope_timer: 9 | 10 | enabled: false 11 | log_filename: "" # if empty, scope timers output to terminal; otherwise log to file 12 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(rostest REQUIRED) 2 | find_package(mrs_uav_testing REQUIRED) 3 | 4 | add_subdirectory(control_manager) 5 | 6 | add_subdirectory(uav_manager) 7 | 8 | add_subdirectory(estimation_manager) 9 | 10 | add_subdirectory(gain_manager) 11 | 12 | add_subdirectory(constraint_manager) 13 | -------------------------------------------------------------------------------- /config/private/constraint_manager/constraint_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | constraint_manager: 4 | 5 | rate: 10.0 # [Hz] 6 | diagnostics_rate: 1.0 # [Hz] 7 | 8 | scope_timer: 9 | 10 | enabled: false 11 | log_filename: "" # if empty, scope timers output to terminal; otherwise log to file 12 | -------------------------------------------------------------------------------- /config/public/estimation_manager/estimation_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | debug_topics: 6 | input: true 7 | output: true 8 | state: true 9 | covariance: false 10 | innovation: false 11 | correction: true 12 | correction_delay: false 13 | diagnostics: false 14 | -------------------------------------------------------------------------------- /test/control_manager/rc_remote_control/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | state_estimators: [ 6 | "gps_baro", 7 | ] 8 | 9 | initial_state_estimator: "gps_baro" 10 | 11 | constraint_manager: 12 | 13 | default_constraints: 14 | gps_garmin: "medium" 15 | gps_baro: "medium" 16 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_start_stop_resume/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | state_estimators: [ 6 | "gps_baro", 7 | ] 8 | 9 | initial_state_estimator: "gps_baro" 10 | 11 | uav_manager: 12 | 13 | takeoff: 14 | 15 | after_takeoff: 16 | controller: "Se3Controller" 17 | -------------------------------------------------------------------------------- /test/uav_manager/landing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/bumper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/land_home/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/land_there/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/eland_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/goto_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/hover_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/max_height_check/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/max_throttle_check/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/min_height_check/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/eland_control_error/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/eland_innovation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/goto_fcu_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/rc_remote_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/reference_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/reference_topic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_topic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_fail/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/constraint_manager/constraints_override/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/bumper_with_rc_remote/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_rc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/failsafe_control_error/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/goto_altitude_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/goto_relative_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/transform_pose_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/velocity_reference_topic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/estimation_manager/estimator_switching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_succeed/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/compile_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG 6 | trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR 7 | 8 | # build the package 9 | catkin build --this -DMRS_ENABLE_TESTING=1 --no-deps -j8 # it has to be fully built normally before building with --catkin-make-args tests 10 | catkin build --this -DMRS_ENABLE_TESTING=1 --no-deps --catkin-make-args tests -j8 11 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_relative_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_start_stop_resume/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/transform_reference_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/transform_vector3_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/validate_reference_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/land_home_moving_takeoff_frame/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_output/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_fail/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_succeed/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/reference_service_local_origin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/control_manager/validate_reference_array_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_offboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 2 | 3 | catkin_add_executable_with_gtest(test_${TEST_NAME} 4 | test.cpp 5 | ) 6 | 7 | target_link_libraries(test_${TEST_NAME} 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(test_${TEST_NAME} 12 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 13 | ${catkin_EXPORTED_TARGETS} 14 | ) 15 | 16 | add_rostest(${TEST_NAME}.test) 17 | -------------------------------------------------------------------------------- /.github/workflows/ros_package_build.yml: -------------------------------------------------------------------------------- 1 | name: ros_package_build 2 | 3 | on: 4 | 5 | push: 6 | branches: [ master ] 7 | 8 | paths-ignore: 9 | - '**/README.md' 10 | - 'test/**' 11 | 12 | workflow_dispatch: 13 | 14 | concurrency: 15 | group: ${{ github.ref }} 16 | cancel-in-progress: true 17 | 18 | jobs: 19 | 20 | build: 21 | uses: ctu-mrs/ci_scripts/.github/workflows/ros_package_build.yml@master 22 | secrets: 23 | PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} 24 | -------------------------------------------------------------------------------- /test/uav_manager/land_home_moving_takeoff_frame/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | # loaded state estimator plugins 6 | # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, ground_truth, dummy 7 | state_estimators: [ 8 | "gps_garmin", 9 | "gps_baro", 10 | # "rtk", 11 | # "ground_truth", 12 | # "dummy" 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | -------------------------------------------------------------------------------- /config/private/uav_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | uav_manager: 4 | 5 | null_tracker: "NullTracker" 6 | 7 | diagnostics: 8 | 9 | rate: 1.0 # [Hz] 10 | 11 | scope_timer: 12 | 13 | enabled: false 14 | log_filename: "" # if empty, scope timers output to terminal; otherwise log to file 15 | 16 | midair_activation: 17 | 18 | rate: 100 # [Hz] 19 | 20 | during_activation: 21 | controller: "MidairActivationController" 22 | tracker: "MidairActivationTracker" 23 | -------------------------------------------------------------------------------- /.github/workflows/ros_build_test.yml: -------------------------------------------------------------------------------- 1 | name: ros_build_test 2 | 3 | on: 4 | 5 | push: 6 | branches: [ devel ] 7 | 8 | paths-ignore: 9 | - '**/README.md' 10 | - 'test/**' 11 | 12 | pull_request: 13 | branches: [ master ] 14 | 15 | workflow_dispatch: 16 | 17 | concurrency: 18 | group: ${{ github.ref }} 19 | cancel-in-progress: true 20 | 21 | jobs: 22 | 23 | build: 24 | uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master 25 | secrets: 26 | PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} 27 | -------------------------------------------------------------------------------- /config/private/estimation_manager/estimation_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | scope_timer: 6 | enabled: false 7 | 8 | # names of used frames are specified here 9 | frame_id: 10 | fcu: "fcu" # the body frame of the UAV where the flight control unit resides (default: "fcu") 11 | fcu_untilted: "fcu_untilted" # the body frame with zero tilt angles default: ("fcu_untilted") 12 | rtk_antenna: "rtk_antenna" # the body frame with zero tilt angles default: ("fcu_untilted") 13 | 14 | rate: 15 | diagnostics: 10 # [Hz] 16 | -------------------------------------------------------------------------------- /config/public/active_estimators.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | # loaded state estimator plugins 6 | # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, ground_truth, dummy 7 | state_estimators: [ 8 | "gps_garmin", 9 | "gps_baro", 10 | # "rtk", 11 | # "ground_truth", 12 | # "dummy" 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 17 | -------------------------------------------------------------------------------- /test/control_manager/bumper_with_rc_remote/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | state_estimators: [ 6 | "gps_baro", 7 | ] 8 | 9 | initial_state_estimator: "gps_baro" 10 | 11 | constraint_manager: 12 | 13 | default_constraints: 14 | gps_garmin: "medium" 15 | gps_baro: "medium" 16 | 17 | control_manager: 18 | 19 | rc_joystick: 20 | 21 | enabled: true 22 | # activation channel 23 | channel_number: 6 # indexed from 0 24 | 25 | horizontal_speed: 4.00 # [m/s] 26 | vertical_speed: 4.00 # [m/s] 27 | heading_rate: 1.00 # [rad/s] 28 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public FailedTakeoffTest { 6 | 7 | public: 8 | Tester() : FailedTakeoffTest(){}; 9 | }; 10 | 11 | TEST(TESTSuite, test) { 12 | 13 | Tester tester; 14 | 15 | bool result = tester.test(); 16 | 17 | if (result) { 18 | GTEST_SUCCEED(); 19 | } else { 20 | GTEST_FAIL(); 21 | } 22 | } 23 | 24 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 25 | 26 | ros::init(argc, argv, "test"); 27 | 28 | testing::InitGoogleTest(&argc, argv); 29 | 30 | return RUN_ALL_TESTS(); 31 | } 32 | 33 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public FailedTakeoffTest { 6 | 7 | public: 8 | Tester() : FailedTakeoffTest(){}; 9 | }; 10 | 11 | TEST(TESTSuite, test) { 12 | 13 | Tester tester; 14 | 15 | bool result = tester.test(); 16 | 17 | if (result) { 18 | GTEST_SUCCEED(); 19 | } else { 20 | GTEST_FAIL(); 21 | } 22 | } 23 | 24 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 25 | 26 | ros::init(argc, argv, "test"); 27 | 28 | testing::InitGoogleTest(&argc, argv); 29 | 30 | return RUN_ALL_TESTS(); 31 | } 32 | 33 | -------------------------------------------------------------------------------- /include/mrs_uav_managers/estimation_manager/private_handlers.h: -------------------------------------------------------------------------------- 1 | #ifndef ESTIMATION_MANAGER_PRIVATE_HANDLERS_H 2 | #define ESTIMATION_MANAGER_PRIVATE_HANDLERS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace mrs_uav_managers 8 | { 9 | 10 | namespace estimation_manager 11 | { 12 | 13 | typedef boost::function loadConfigFile_t; 14 | 15 | struct PrivateHandlers_t 16 | { 17 | std::unique_ptr param_loader; 18 | loadConfigFile_t loadConfigFile; 19 | }; 20 | 21 | } // namespace estimation_manager 22 | 23 | } // namespace mrs_uav_managers 24 | 25 | #endif // ESTIMATION_MANAGER_PRIVATE_HANDLERS_H 26 | -------------------------------------------------------------------------------- /test/control_manager/eland_innovation/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "passthrough", 13 | ] 14 | 15 | initial_state_estimator: "passthrough" # will be used as the first state estimator 16 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 17 | -------------------------------------------------------------------------------- /test/uav_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(takeoff) 2 | 3 | add_subdirectory(takeoff_fail_cause_controller) 4 | 5 | add_subdirectory(takeoff_fail_cause_tracker) 6 | 7 | add_subdirectory(midair_fail_cause_enabling_output) 8 | 9 | add_subdirectory(midair_fail_cause_enabling_offboard) 10 | 11 | add_subdirectory(midair_fail_cause_wrong_tracker) 12 | 13 | add_subdirectory(midair_fail_cause_wrong_controller) 14 | 15 | add_subdirectory(landing) 16 | 17 | add_subdirectory(land_home) 18 | 19 | add_subdirectory(land_home_moving_takeoff_frame) 20 | 21 | add_subdirectory(land_there) 22 | 23 | add_subdirectory(min_height_check) 24 | 25 | add_subdirectory(max_height_check) 26 | 27 | add_subdirectory(max_throttle_check) 28 | -------------------------------------------------------------------------------- /config/private/estimators.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | gps_garmin: 6 | address: "mrs_uav_state_estimators/GpsGarmin" 7 | gps_baro: 8 | address: "mrs_uav_state_estimators/GpsBaro" 9 | rtk: 10 | address: "mrs_uav_state_estimators/Rtk" 11 | rtk_garmin: 12 | address: "mrs_uav_state_estimators/RtkGarmin" 13 | passthrough: 14 | address: "mrs_uav_state_estimators/Passthrough" 15 | f9p: 16 | address: "mrs_uav_state_estimators/F9P" 17 | ground_truth: 18 | address: "mrs_uav_state_estimators/GroundTruth" 19 | dummy: 20 | address: "mrs_uav_state_estimators/Dummy" 21 | garmin_agl: 22 | address: "mrs_uav_state_estimators/GarminAgl" 23 | -------------------------------------------------------------------------------- /include/mrs_uav_managers/control_manager/private_handlers.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANAGER_PRIVATE_HANDLERS_H 2 | #define CONTROL_MANAGER_PRIVATE_HANDLERS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace mrs_uav_managers 8 | { 9 | 10 | namespace control_manager 11 | { 12 | 13 | typedef boost::function loadConfigFile_t; 14 | 15 | struct PrivateHandlers_t 16 | { 17 | loadConfigFile_t loadConfigFile; 18 | std::unique_ptr param_loader; 19 | std::string name_space; 20 | std::string runtime_name; 21 | }; 22 | 23 | } // namespace control_manager 24 | 25 | } // namespace mrs_uav_managers 26 | 27 | #endif // CONTROL_MANAGER_PRIVATE_HANDLERS_H 28 | -------------------------------------------------------------------------------- /test/control_manager/bumper/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/uav_manager/land_there/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/hover_service/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/uav_manager/max_height_check/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/uav_manager/min_height_check/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/eland_innovation/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/rc_remote_control/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_output/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -1, -1, 23 | -1, 1, 24 | 1, 1, 25 | 1, -1 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 1.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/bumper_with_rc_remote/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_rc/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/estimation_manager/estimator_switching/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_service/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_start_stop_resume/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: false 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | uav_manager: 19 | 20 | takeoff: 21 | 22 | during_takeoff: 23 | # some nonexisting tracker 24 | tracker: "WrongTracker" 25 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | uav_manager: 19 | 20 | takeoff: 21 | 22 | during_takeoff: 23 | # some nonexisting controller 24 | controller: "WrongController" 25 | -------------------------------------------------------------------------------- /test/uav_manager/max_height_check/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | uav_manager: 19 | 20 | max_height_checking: 21 | 22 | enabled: true 23 | rate: 10.0 # [Hz] 24 | safety_height_offset: 0.25 # how much lower to descend below the max height 25 | -------------------------------------------------------------------------------- /test/estimation_manager/estimator_switching/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_baro", 13 | "gps_garmin", 14 | "rtk", 15 | "rtk_garmin", 16 | ] 17 | 18 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 19 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 20 | 21 | uav_manager: 22 | 23 | midair_activation: 24 | 25 | after_activation: 26 | controller: "Se3Controller" 27 | tracker: "MpcTracker" 28 | -------------------------------------------------------------------------------- /test/gather_coverage.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | while [ ! -e ".catkin_tools" ]; do 4 | cd .. 5 | if [[ `pwd` == "/" ]]; then 6 | # we reached the root and didn't find the build/COLCON_IGNORE file - that's a fail! 7 | echo "$0: could not find the root of the current workspace". 8 | return 1 9 | fi 10 | done 11 | 12 | WORKSPACE_NAME=${PWD##*/} 13 | 14 | cd build 15 | 16 | lcov --capture --directory . --output-file coverage.info 17 | lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed 18 | lcov --extract coverage.info.removed "*/${WORKSPACE_NAME}/src/*" --output-file coverage.info.cleaned 19 | genhtml --title "MRS UAV System - Test coverage report" --demangle-cpp --legend --frames --show-details -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log 20 | 21 | COVERAGE_PCT=`cat /tmp/genhtml.log | tail -n 1 | awk '{print $2}'` 22 | 23 | echo "Coverage: $COVERAGE_PCT" 24 | 25 | xdg-open coverage_html/index.html 26 | -------------------------------------------------------------------------------- /test/uav_manager/min_height_check/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | uav_manager: 19 | 20 | min_height_checking: 21 | 22 | enabled: true 23 | rate: 10.0 # [Hz] 24 | min_height: 1.0 # [m] 25 | safety_height_offset: 0.25 # how much higher to ascend above the min height 26 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_rc/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_baro", 13 | ] 14 | 15 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 16 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | control_manager: 19 | 20 | safety: 21 | 22 | escalating_failsafe: 23 | 24 | # how often does it allow to escalate 25 | timeout: 2.0 # [s] 26 | 27 | ehover: true 28 | eland: true 29 | failsafe: true 30 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_service/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_baro", 13 | ] 14 | 15 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 16 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | control_manager: 19 | 20 | safety: 21 | 22 | escalating_failsafe: 23 | 24 | # how often does it allow to escalate 25 | timeout: 2.0 # [s] 26 | 27 | ehover: true 28 | eland: true 29 | failsafe: true 30 | -------------------------------------------------------------------------------- /test/control_manager/eland_control_error/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | control_manager: 19 | 20 | MpcController: 21 | eland_threshold: 0.2 # [m], position error triggering eland 22 | failsafe_threshold: 0.0 # [m], position error triggering failsafe land 23 | odometry_innovation_threshold: 0.0 # [m], position odometry innovation threshold 24 | -------------------------------------------------------------------------------- /test/control_manager/failsafe_control_error/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | ] 14 | 15 | initial_state_estimator: "gps_garmin" # will be used as the first state estimator 16 | agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) 17 | 18 | control_manager: 19 | 20 | MpcController: 21 | eland_threshold: 0.0 # [m], position error triggering eland 22 | failsafe_threshold: 0.2 # [m], position error triggering failsafe land 23 | odometry_innovation_threshold: 0.0 # [m], position odometry innovation threshold 24 | -------------------------------------------------------------------------------- /config/private/trackers.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | control_manager: 4 | 5 | MpcTracker: 6 | address: "mrs_uav_trackers/MpcTracker" 7 | namespace: "mpc_tracker" 8 | human_switchable: true 9 | 10 | LandoffTracker: 11 | namespace: "landoff_tracker" 12 | address: "mrs_uav_trackers/LandoffTracker" 13 | 14 | LineTracker: 15 | namespace: "line_tracker" 16 | address: "mrs_uav_trackers/LineTracker" 17 | 18 | JoyTracker: 19 | namespace: "joy_tracker" 20 | address: "mrs_uav_trackers/JoyTracker" 21 | 22 | SpeedTracker: 23 | namespace: "speed_tracker" 24 | address: "mrs_uav_trackers/SpeedTracker" 25 | 26 | FlipTracker: 27 | namespace: "flip_tracker" 28 | address: "mrs_uav_trackers/FlipTracker" 29 | 30 | MidairActivationTracker: 31 | namespace: "midair_activation_tracker" 32 | address: "mrs_uav_trackers/MidairActivationTracker" 33 | 34 | NullTracker: 35 | namespace: "null_tracker" 36 | address: "mrs_uav_managers/NullTracker" 37 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrs_uav_managers 5 | 1.0.4 6 | The mrs_uav_managers package 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | BSD 3-Clause 12 | 13 | catkin 14 | 15 | cmake_modules 16 | roscpp 17 | rospy 18 | nodelet 19 | geometry_msgs 20 | std_msgs 21 | nav_msgs 22 | mrs_msgs 23 | visualization_msgs 24 | mrs_lib 25 | tf2 26 | tf2_ros 27 | tf2_geometry_msgs 28 | rostest 29 | mrs_uav_testing 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /config/public/gain_manager/gain_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | gain_manager: 4 | 5 | estimator_types: [ 6 | "gps_garmin", 7 | "gps_baro", 8 | "rtk", 9 | "rtk_garmin", 10 | "other", 11 | "ground_truth", 12 | "passthrough", 13 | "f9p", 14 | ] 15 | 16 | gains: [ 17 | "supersoft", 18 | "soft", 19 | ] 20 | 21 | # list of allowed gains per odometry mode 22 | allowed_gains: 23 | gps_garmin: ["supersoft", "soft"] 24 | gps_baro: ["supersoft", "soft"] 25 | rtk: ["supersoft", "soft"] 26 | rtk_garmin: ["supersoft", "soft"] 27 | other: ["supersoft", "soft"] 28 | ground_truth: ["supersoft", "soft"] 29 | passthrough: ["supersoft", "soft"] 30 | f9p: ["supersoft", "soft"] 31 | 32 | # those gains will be used automatically when a localization mode switches 33 | # and the current gains are not in the allowed list (next paragraphs) 34 | default_gains: 35 | gps_garmin: "soft" 36 | gps_baro: "soft" 37 | rtk: "soft" 38 | rtk_garmin: "soft" 39 | other: "supersoft" 40 | ground_truth: "soft" 41 | passthrough: "soft" 42 | f9p: "soft" 43 | -------------------------------------------------------------------------------- /config/public/gain_manager/gains.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | gain_manager: 4 | 5 | supersoft: 6 | 7 | horizontal: 8 | kp: 3.0 9 | kv: 2.0 10 | ka: 1.0 11 | 12 | kib: 0.1 13 | kib_lim: 10.0 14 | 15 | kiw: 0.1 16 | kiw_lim: 10.0 17 | 18 | vertical: 19 | kp: 15.0 20 | kv: 8.0 21 | ka: 1.0 22 | 23 | attitude: 24 | kq_roll_pitch: 5.0 25 | kq_yaw: 5.0 26 | 27 | mass_estimator: 28 | km: 0.5 29 | 30 | # the maximum estimated uav mass difference 31 | km_lim: 20.0 # [kg, at least 1/2 of the UAVs mass to allow landing detection] 32 | 33 | soft: 34 | 35 | horizontal: 36 | kp: 6.0 37 | kv: 4.0 38 | ka: 1.0 39 | 40 | kib: 0.1 41 | kib_lim: 10.0 42 | 43 | kiw: 0.1 44 | kiw_lim: 10.0 45 | 46 | vertical: 47 | kp: 15.0 48 | kv: 8.0 49 | ka: 1.0 50 | 51 | attitude: 52 | kq_roll_pitch: 5.0 53 | kq_yaw: 5.0 54 | 55 | mass_estimator: 56 | km: 0.5 57 | 58 | # the maximum estimated uav mass difference 59 | km_lim: 20.0 # [kg, at least 1/2 of the UAVs mass to allow landing detection] 60 | -------------------------------------------------------------------------------- /config/public/constraint_manager/constraint_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | constraint_manager: 4 | 5 | estimator_types: [ 6 | "gps_garmin", 7 | "gps_baro", 8 | "rtk", 9 | "rtk_garmin", 10 | "other", 11 | "ground_truth", 12 | "passthrough", 13 | "f9p", 14 | ] 15 | 16 | constraints: [ 17 | "slow", 18 | "medium", 19 | "fast", 20 | ] 21 | 22 | # list of allowed constraints per odometry mode 23 | allowed_constraints: 24 | gps_garmin: ["slow", "medium", "fast"] 25 | gps_baro: ["slow", "medium", "fast"] 26 | rtk: ["slow", "medium", "fast"] 27 | rtk_garmin: ["slow", "medium", "fast"] 28 | other: ["slow"] 29 | ground_truth: ["slow", "medium", "fast"] 30 | passthrough: ["slow", "medium", "fast"] 31 | f9p: ["slow", "medium", "fast"] 32 | 33 | # those constraints will be used automatically when a localization mode switches 34 | # and the current constraints are not in the allowed list (next paragraphs) 35 | default_constraints: 36 | gps_garmin: "slow" 37 | gps_baro: "slow" 38 | rtk: "slow" 39 | rtk_garmin: "slow" 40 | other: "slow" 41 | ground_truth: "slow" 42 | passthrough: "slow" 43 | f9p: "slow" 44 | -------------------------------------------------------------------------------- /test/control_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(goto_service) 2 | 3 | add_subdirectory(goto_fcu_service) 4 | 5 | add_subdirectory(goto_relative_service) 6 | 7 | add_subdirectory(eland_control_error) 8 | 9 | add_subdirectory(eland_innovation) 10 | 11 | add_subdirectory(failsafe_control_error) 12 | 13 | add_subdirectory(eland_service) 14 | 15 | add_subdirectory(escalating_failsafe_service) 16 | 17 | add_subdirectory(escalating_failsafe_rc) 18 | 19 | add_subdirectory(rc_remote_control) 20 | 21 | add_subdirectory(bumper) 22 | 23 | add_subdirectory(bumper_with_rc_remote) 24 | 25 | add_subdirectory(trajectory_topic) 26 | 27 | add_subdirectory(trajectory_start_stop_resume) 28 | 29 | add_subdirectory(velocity_reference_topic) 30 | 31 | add_subdirectory(set_heading_service) 32 | 33 | add_subdirectory(set_heading_relative_service) 34 | 35 | add_subdirectory(goto_altitude_service) 36 | 37 | add_subdirectory(hover_service) 38 | 39 | add_subdirectory(reference_service) 40 | 41 | add_subdirectory(reference_service_local_origin) 42 | 43 | add_subdirectory(reference_topic) 44 | 45 | add_subdirectory(validate_reference_service) 46 | 47 | add_subdirectory(validate_reference_array_service) 48 | 49 | add_subdirectory(transform_reference_service) 50 | 51 | add_subdirectory(transform_pose_service) 52 | 53 | add_subdirectory(transform_vector3_service) 54 | -------------------------------------------------------------------------------- /include/mrs_uav_managers/estimation_manager/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef ESTIMATION_MANAGER_TYPES_H 3 | #define ESTIMATION_MANAGER_TYPES_H 4 | 5 | namespace mrs_uav_managers 6 | { 7 | 8 | namespace estimation_manager 9 | { 10 | /*//{ typedef */ 11 | 12 | typedef enum 13 | { 14 | 15 | AXIS_X, 16 | AXIS_Y, 17 | AXIS_Z 18 | 19 | } AxisId_t; 20 | 21 | typedef enum 22 | { 23 | 24 | POSITION, 25 | VELOCITY, 26 | ACCELERATION, 27 | ACCELERATION_BIAS, 28 | ACCELERATION_UNBIASED 29 | 30 | } StateId_t; 31 | const int n_StateId_t = 5; 32 | 33 | typedef enum 34 | { 35 | 36 | UNINITIALIZED_STATE, 37 | INITIALIZED_STATE, 38 | READY_STATE, 39 | STARTED_STATE, 40 | RUNNING_STATE, 41 | STOPPED_STATE, 42 | ERROR_STATE 43 | 44 | } SMStates_t; 45 | 46 | typedef enum 47 | { 48 | 49 | METERS, 50 | DEGREES 51 | 52 | } UtmUnits_t; 53 | 54 | /*//}*/ 55 | 56 | namespace sm 57 | { 58 | // clang-format off 59 | const std::vector state_names = { 60 | "UNINITIALIZED_STATE", 61 | "INITIALIZED_STATE", 62 | "READY_STATE", 63 | "STARTED_STATE", 64 | "RUNNING_STATE", 65 | "STOPPED_STATE", 66 | "ERROR_STATE" 67 | }; 68 | // clang-format on 69 | 70 | } // namespace sm 71 | 72 | } // namespace estimation_manager 73 | 74 | } // namespace mrs_uav_managers 75 | 76 | #endif // ESTIMATION_MANAGER_TYPES_H 77 | -------------------------------------------------------------------------------- /scripts/get_public_params.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import rospkg 4 | import os 5 | 6 | class ParamsGetter: 7 | 8 | def __init__(self): 9 | 10 | package_name = "mrs_uav_managers" 11 | 12 | rospack = rospkg.RosPack() 13 | 14 | package_path = rospack.get_path(package_name) 15 | 16 | file_paths = [] 17 | 18 | for path, subdirs, files in os.walk(package_path + "/config/public/"): 19 | for name in files: 20 | if name.endswith(".yaml") or name.endswith(".yml"): 21 | file_paths.append(os.path.join(path, name)) 22 | 23 | for file_path in file_paths: 24 | 25 | if os.path.getsize(file_path) == 0: 26 | continue 27 | 28 | with open(file_path, 'r') as file: 29 | try: 30 | contents = file.read() 31 | print("######################################") 32 | print("# The following section was taken from:") 33 | print("# {}".format(file_path)) 34 | print("") 35 | print(contents) 36 | except: 37 | print("There was a problem while opening the file '{}'".format(file_path)) 38 | break 39 | 40 | if __name__ == '__main__': 41 | params_getter = ParamsGetter() 42 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_output/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (success) { 30 | ROS_ERROR("[%s]: midair activation succeeded, which should not happen: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | return true; 36 | } 37 | 38 | 39 | TEST(TESTSuite, test) { 40 | 41 | Tester tester; 42 | 43 | bool result = tester.test(); 44 | 45 | if (result) { 46 | GTEST_SUCCEED(); 47 | } else { 48 | GTEST_FAIL(); 49 | } 50 | } 51 | 52 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 53 | 54 | ros::init(argc, argv, "test"); 55 | 56 | testing::InitGoogleTest(&argc, argv); 57 | 58 | return RUN_ALL_TESTS(); 59 | } 60 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_offboard/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (success) { 30 | ROS_ERROR("[%s]: midair activation succeeded, which should not happen: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | return true; 36 | } 37 | 38 | 39 | TEST(TESTSuite, test) { 40 | 41 | Tester tester; 42 | 43 | bool result = tester.test(); 44 | 45 | if (result) { 46 | GTEST_SUCCEED(); 47 | } else { 48 | GTEST_FAIL(); 49 | } 50 | } 51 | 52 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 53 | 54 | ros::init(argc, argv, "test"); 55 | 56 | testing::InitGoogleTest(&argc, argv); 57 | 58 | return RUN_ALL_TESTS(); 59 | } 60 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public EscalatingFailsafeTest { 6 | 7 | public: 8 | Tester(); 9 | 10 | mrs_lib::ServiceClientHandler sch_esc_failsafe_; 11 | 12 | std::optional> escalatingFailsafe(); 13 | }; 14 | 15 | Tester::Tester() { 16 | 17 | sch_esc_failsafe_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/failsafe_escalating"); 18 | } 19 | 20 | std::optional> Tester::escalatingFailsafe() { 21 | 22 | { 23 | std_srvs::Trigger srv; 24 | 25 | { 26 | bool service_call = sch_esc_failsafe_.call(srv); 27 | 28 | if (!service_call || !srv.response.success) { 29 | return {{false, "escalating failsafe service call failed"}}; 30 | } 31 | } 32 | } 33 | 34 | return {{true, "service called"}}; 35 | } 36 | 37 | TEST(TESTSuite, test) { 38 | 39 | Tester tester; 40 | 41 | bool result = tester.test(); 42 | 43 | if (result) { 44 | GTEST_SUCCEED(); 45 | } else { 46 | GTEST_FAIL(); 47 | } 48 | } 49 | 50 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 51 | 52 | ros::init(argc, argv, "test"); 53 | 54 | testing::InitGoogleTest(&argc, argv); 55 | 56 | return RUN_ALL_TESTS(); 57 | } 58 | -------------------------------------------------------------------------------- /config/public/controllers.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | control_manager: 4 | 5 | Se3Controller: 6 | eland_threshold: 1.5 # [m], position error triggering eland 7 | failsafe_threshold: 2.5 # [m], position error triggering failsafe land 8 | odometry_innovation_threshold: 1.5 # [m], position odometry innovation threshold 9 | 10 | MpcController: 11 | eland_threshold: 3.5 # [m], position error triggering eland 12 | failsafe_threshold: 4.5 # [m], position error triggering failsafe land 13 | odometry_innovation_threshold: 2.5 # [m], position odometry innovation threshold 14 | 15 | FailsafeController: 16 | eland_threshold: 0.0 # [m], position error triggering eland 17 | failsafe_threshold: 0.0 # [m], position error triggering failsafe land 18 | odometry_innovation_threshold: 0.0 # [m], position odometry innovation threshold 19 | 20 | EmergencyController: 21 | eland_threshold: 3.0 # [m], position error triggering eland 22 | failsafe_threshold: 4.0 # [m], position error triggering failsafe land 23 | odometry_innovation_threshold: 3.0 # [m], position odometry innovation threshold 24 | 25 | MidairActivationController: 26 | eland_threshold: 3.0 # [m], position error triggering eland 27 | failsafe_threshold: 4.0 # [m], position error triggering failsafe land 28 | odometry_innovation_threshold: 3.0 # [m], position odometry innovation threshold 29 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | auto [success, message] = uh->takeoff(); 27 | 28 | if (!success) { 29 | ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 30 | return false; 31 | } 32 | 33 | this->sleep(5.0); 34 | 35 | if (uh->isFlyingNormally()) { 36 | return true; 37 | } else { 38 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 39 | return false; 40 | } 41 | } 42 | 43 | 44 | TEST(TESTSuite, test) { 45 | 46 | Tester tester; 47 | 48 | bool result = tester.test(); 49 | 50 | if (result) { 51 | GTEST_SUCCEED(); 52 | } else { 53 | GTEST_FAIL(); 54 | } 55 | } 56 | 57 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 58 | 59 | ros::init(argc, argv, "test"); 60 | 61 | testing::InitGoogleTest(&argc, argv); 62 | 63 | return RUN_ALL_TESTS(); 64 | } 65 | -------------------------------------------------------------------------------- /test/uav_manager/landing/landing.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/eland_service/eland_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/goto_service/goto_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/goto_fcu_service/goto_fcu_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/reference_topic/reference_topic.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/uav_manager/max_throttle_check/max_throttle_check.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/reference_service/reference_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_service/set_heading_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_fail/set_gains_should_fail.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/constraint_manager/constraints_override/constraints_override.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/goto_altitude_service/goto_altitude_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/goto_relative_service/goto_relative_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/transform_pose_service/transform_pose_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/velocity_reference_topic/velocity_reference_topic.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_succeed/set_gains_should_succeed.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/transform_vector3_service/transform_vector3_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/validate_reference_service/validate_reference_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_offboard/midair_fail_cause_enabling_offboard.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_fail/set_constraints_should_fail.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/transform_reference_service/transform_reference_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_succeed/set_constraints_should_succeed.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/reference_service_local_origin/reference_service_local_origin.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/validate_reference_array_service/validate_reference_array_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /test/control_manager/bumper/bumper.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/uav_manager/land_home/land_home.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff/takeoff.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/control_manager/hover_service/hover_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/control_manager/eland_control_error/eland_control_error.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Multi-robot Systems (MRS) group at Czech Technical University in Prague 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /test/control_manager/failsafe_control_error/failsafe_control_error.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_relative_service/set_heading_relative_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_topic/trajectory_topic.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_tracker/midair_fail_cause_wrong_tracker.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_enabling_output/midair_fail_cause_enabling_output.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_controller/midair_fail_cause_wrong_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /test/control_manager/eland_innovation/eland_innovation.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/uav_manager/land_there/land_there.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/uav_manager/max_height_check/max_height_check.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/control_manager/goto_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoAbs(0, 0, 2.0, 0); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/control_manager/goto_fcu_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoFcu(3, 2, 1, 1); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto_fcu failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/control_manager/goto_relative_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoRel(5, 10, 2.0, 1.2); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/control_manager/rc_remote_control/rc_remote_control.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /test/control_manager/reference_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoReference(0, 0, 2.0, 0, ""); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto reference failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/control_manager/reference_topic/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoReferenceTopic(0, 0, 2.0, 0, ""); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto reference failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/estimation_manager/estimator_switching/estimator_switching.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /test/uav_manager/min_height_check/min_height_check.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /include/mrs_uav_managers/estimation_manager/common_handlers.h: -------------------------------------------------------------------------------- 1 | #ifndef ESTIMATION_MANAGER_COMMON_HANDLERS_H 2 | #define ESTIMATION_MANAGER_COMMON_HANDLERS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace mrs_uav_managers 9 | { 10 | 11 | namespace estimation_manager 12 | { 13 | 14 | struct CommonFrames_t 15 | { 16 | 17 | std::string fcu; 18 | std::string ns_fcu; 19 | std::string fcu_untilted; 20 | std::string ns_fcu_untilted; 21 | std::string rtk_antenna; 22 | std::string ns_rtk_antenna; 23 | std::string amsl; 24 | std::string ns_amsl; 25 | }; 26 | 27 | struct WorldOrigin_t 28 | { 29 | double x; 30 | double y; 31 | }; 32 | 33 | struct DebugTopics_t 34 | { 35 | bool input; 36 | bool output; 37 | bool state; 38 | bool covariance; 39 | bool innovation; 40 | bool diag; 41 | bool correction; 42 | bool corr_delay; 43 | }; 44 | 45 | struct ScopeTimer_t 46 | { 47 | bool enabled; 48 | std::shared_ptr logger; 49 | }; 50 | 51 | struct CommonHandlers_t 52 | { 53 | 54 | std::string nodelet_name; 55 | std::string package_name; 56 | std::string uav_name; 57 | CommonFrames_t frames; 58 | std::shared_ptr transformer; 59 | double desired_uav_state_rate; 60 | double desired_diagnostics_rate; 61 | WorldOrigin_t world_origin; 62 | ScopeTimer_t scope_timer; 63 | DebugTopics_t debug_topics; 64 | }; 65 | 66 | } // namespace estimation_manager 67 | 68 | } // namespace mrs_uav_managers 69 | 70 | #endif // COMMON_HANDLERS_H 71 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_tracker/takeoff_fail_cause_tracker.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/uav_manager/takeoff_fail_cause_controller/takeoff_fail_cause_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /test/control_manager/reference_service_local_origin/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoReference(3.2, -1.3, 1.2, -2.3, _uav_name_ + "/local_origin"); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto reference failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | this->sleep(5.0); 45 | 46 | if (uh->isFlyingNormally()) { 47 | return true; 48 | } else { 49 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | TEST(TESTSuite, test) { 55 | 56 | Tester tester; 57 | 58 | bool result = tester.test(); 59 | 60 | if (result) { 61 | GTEST_SUCCEED(); 62 | } else { 63 | GTEST_FAIL(); 64 | } 65 | } 66 | 67 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 68 | 69 | ros::init(argc, argv, "test"); 70 | 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /test/control_manager/trajectory_start_stop_resume/trajectory_start_stop_resume.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_tracker/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | // | ---------- call midair activation in async task ---------- | 27 | 28 | { 29 | auto [success, message] = uh->activateMidAir(); 30 | 31 | if (success) { 32 | ROS_ERROR("[%s]: midair activation succeeded, this should not happen: '%s'", ros::this_node::getName().c_str(), message.c_str()); 33 | return false; 34 | } 35 | } 36 | 37 | // | -------------------------- wait -------------------------- | 38 | 39 | sleep(1.0); 40 | 41 | // | -------------- check if we are in emergency -------------- | 42 | 43 | if (!uh->isFlyingNormally() && uh->getActiveController() == "EmergencyController" && uh->getActiveTracker() == "LandoffTracker") { 44 | return true; 45 | } else { 46 | ROS_ERROR("[%s]: not in the ehover state", ros::this_node::getName().c_str()); 47 | return false; 48 | } 49 | } 50 | 51 | TEST(TESTSuite, test) { 52 | 53 | Tester tester; 54 | 55 | bool result = tester.test(); 56 | 57 | if (result) { 58 | GTEST_SUCCEED(); 59 | } else { 60 | GTEST_FAIL(); 61 | } 62 | } 63 | 64 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 65 | 66 | ros::init(argc, argv, "test"); 67 | 68 | testing::InitGoogleTest(&argc, argv); 69 | 70 | return RUN_ALL_TESTS(); 71 | } 72 | -------------------------------------------------------------------------------- /test/uav_manager/midair_fail_cause_wrong_controller/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | // | ---------- call midair activation in async task ---------- | 27 | 28 | { 29 | auto [success, message] = uh->activateMidAir(); 30 | 31 | if (success) { 32 | ROS_ERROR("[%s]: midair activation succeeded, this should not happen: '%s'", ros::this_node::getName().c_str(), message.c_str()); 33 | return false; 34 | } 35 | } 36 | 37 | // | -------------------------- wait -------------------------- | 38 | 39 | sleep(1.0); 40 | 41 | // | -------------- check if we are in emergency -------------- | 42 | 43 | if (!uh->isFlyingNormally() && uh->getActiveController() == "EmergencyController" && uh->getActiveTracker() == "LandoffTracker") { 44 | return true; 45 | } else { 46 | ROS_ERROR("[%s]: not in the ehover state", ros::this_node::getName().c_str()); 47 | return false; 48 | } 49 | } 50 | 51 | TEST(TESTSuite, test) { 52 | 53 | Tester tester; 54 | 55 | bool result = tester.test(); 56 | 57 | if (result) { 58 | GTEST_SUCCEED(); 59 | } else { 60 | GTEST_FAIL(); 61 | } 62 | } 63 | 64 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 65 | 66 | ros::init(argc, argv, "test"); 67 | 68 | testing::InitGoogleTest(&argc, argv); 69 | 70 | return RUN_ALL_TESTS(); 71 | } 72 | -------------------------------------------------------------------------------- /nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | UavManager nodelet 4 | 5 | 6 | 7 | 8 | 9 | ControlManager nodelet 10 | 11 | 12 | 13 | 14 | 15 | ConstraintManager nodelet 16 | 17 | 18 | 19 | 20 | 21 | GainManager nodelet 22 | 23 | 24 | 25 | 26 | 27 | TfManager nodelet 28 | 29 | 30 | 31 | 32 | 33 | EstimationManager nodelet 34 | 35 | 36 | 37 | 38 | 39 | TransformManager nodelet 40 | 41 | 42 | -------------------------------------------------------------------------------- /test/control_manager/goto_altitude_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoAltitude(8.0); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: goto altitude service failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | { 45 | auto [success, message] = uh->gotoAltitude(3.0); 46 | 47 | if (!success) { 48 | ROS_ERROR("[%s]: goto altitude service failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 49 | return false; 50 | } 51 | } 52 | 53 | this->sleep(5.0); 54 | 55 | if (uh->isFlyingNormally()) { 56 | return true; 57 | } else { 58 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 59 | return false; 60 | } 61 | } 62 | 63 | TEST(TESTSuite, test) { 64 | 65 | Tester tester; 66 | 67 | bool result = tester.test(); 68 | 69 | if (result) { 70 | GTEST_SUCCEED(); 71 | } else { 72 | GTEST_FAIL(); 73 | } 74 | } 75 | 76 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 77 | 78 | ros::init(argc, argv, "test"); 79 | 80 | testing::InitGoogleTest(&argc, argv); 81 | 82 | return RUN_ALL_TESTS(); 83 | } 84 | -------------------------------------------------------------------------------- /test/control_manager/escalating_failsafe_rc/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | class Tester : public EscalatingFailsafeTest { 8 | 9 | public: 10 | Tester(); 11 | 12 | mrs_lib::PublisherHandler ph_rc_channels_; 13 | 14 | std::optional> escalatingFailsafe(); 15 | 16 | ros::Timer timer_rc_; 17 | void timerRc(const ros::TimerEvent& event); 18 | 19 | mrs_msgs::HwApiRcChannels rc_; 20 | std::mutex mutex_rc_; 21 | }; 22 | 23 | Tester::Tester() { 24 | 25 | ph_rc_channels_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/rc_channels"); 26 | 27 | rc_.channels.push_back(0.5); 28 | rc_.channels.push_back(0.5); 29 | rc_.channels.push_back(0.5); 30 | rc_.channels.push_back(0.5); 31 | rc_.channels.push_back(0.0); 32 | rc_.channels.push_back(0.0); 33 | rc_.channels.push_back(0.0); 34 | rc_.channels.push_back(0.0); 35 | rc_.channels.push_back(0.0); 36 | rc_.channels.push_back(0.0); 37 | 38 | timer_rc_ = nh_.createTimer(ros::Rate(100.0), &Tester::timerRc, this, false, true); 39 | } 40 | 41 | void Tester::timerRc([[maybe_unused]] const ros::TimerEvent& event) { 42 | 43 | { 44 | std::scoped_lock lock(mutex_rc_); 45 | 46 | ph_rc_channels_.publish(rc_); 47 | } 48 | } 49 | 50 | std::optional> Tester::escalatingFailsafe() { 51 | 52 | { 53 | std::scoped_lock lock(mutex_rc_); 54 | 55 | rc_.channels[7] = 1.0; 56 | } 57 | 58 | sleep(0.2); 59 | 60 | { 61 | std::scoped_lock lock(mutex_rc_); 62 | 63 | rc_.channels[7] = 0.0; 64 | } 65 | 66 | return {}; 67 | } 68 | 69 | TEST(TESTSuite, test) { 70 | 71 | Tester tester; 72 | 73 | bool result = tester.test(); 74 | 75 | if (result) { 76 | GTEST_SUCCEED(); 77 | } else { 78 | GTEST_FAIL(); 79 | } 80 | } 81 | 82 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 83 | 84 | ros::init(argc, argv, "test"); 85 | 86 | testing::InitGoogleTest(&argc, argv); 87 | 88 | return RUN_ALL_TESTS(); 89 | } 90 | -------------------------------------------------------------------------------- /include/mrs_uav_managers/agl_estimator.h: -------------------------------------------------------------------------------- 1 | #ifndef MRS_UAV_MANAGERS_AGL_ESTIMATOR_H 2 | #define MRS_UAV_MANAGERS_AGL_ESTIMATOR_H 3 | 4 | /* includes //{ */ 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | //} 21 | 22 | namespace mrs_uav_managers 23 | { 24 | 25 | namespace agl 26 | { 27 | const char type[] = "AGL"; 28 | } 29 | 30 | using namespace estimation_manager; 31 | 32 | class AglEstimator : public Estimator { 33 | 34 | protected: 35 | const std::string package_name_ = "mrs_uav_state_estimators"; 36 | 37 | ros::NodeHandle nh_; 38 | 39 | mrs_msgs::Float64Stamped agl_height_; 40 | mrs_msgs::Float64Stamped agl_height_init_; 41 | mutable std::mutex mtx_agl_height_; 42 | 43 | mrs_msgs::Float64ArrayStamped agl_height_cov_; 44 | mrs_msgs::Float64ArrayStamped agl_height_cov_init_; 45 | mutable std::mutex mtx_agl_height_cov_; 46 | 47 | bool is_override_frame_id_ = false; 48 | 49 | protected: 50 | mutable mrs_lib::PublisherHandler ph_agl_height_; 51 | mutable mrs_lib::PublisherHandler ph_agl_height_cov_; 52 | 53 | public: 54 | AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name) 55 | : Estimator(agl::type, name, frame_id), package_name_(package_name) { 56 | } 57 | 58 | virtual ~AglEstimator(void) { 59 | } 60 | 61 | // virtual methods 62 | virtual mrs_msgs::Float64Stamped getUavAglHeight() const = 0; 63 | virtual std::vector getHeightCovariance() const = 0; 64 | 65 | // implemented methods 66 | void publishAglHeight() const; 67 | void publishCovariance() const; 68 | bool isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr &hw_api_capabilities) const; 69 | }; 70 | 71 | } // namespace mrs_uav_managers 72 | 73 | #endif // MRS_UAV_MANAGERS_AGL_ESTIMATOR_H 74 | -------------------------------------------------------------------------------- /test/control_manager/failsafe_control_error/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoRel(2, 2, 2, 0); 37 | 38 | if (success) { 39 | ROS_ERROR("[%s]: goto succeeded, that should not happen in this case: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | // | ------------ wait for the failsafe to trigger ------------ | 45 | 46 | while (true) { 47 | 48 | if (!ros::ok()) { 49 | return false; 50 | } 51 | 52 | if (!uh->isFlyingNormally() && uh->getActiveController() == "FailsafeController") { 53 | break; 54 | } 55 | 56 | sleep(0.01); 57 | } 58 | 59 | // | -------------------- wait for landing -------------------- | 60 | 61 | while (true) { 62 | 63 | if (!ros::ok()) { 64 | return false; 65 | } 66 | 67 | if (!uh->isOutputEnabled()) { 68 | return true; 69 | } 70 | 71 | sleep(0.01); 72 | } 73 | 74 | return false; 75 | } 76 | 77 | 78 | TEST(TESTSuite, test) { 79 | 80 | Tester tester; 81 | 82 | bool result = tester.test(); 83 | 84 | if (result) { 85 | GTEST_SUCCEED(); 86 | } else { 87 | GTEST_FAIL(); 88 | } 89 | } 90 | 91 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 92 | 93 | ros::init(argc, argv, "test"); 94 | 95 | testing::InitGoogleTest(&argc, argv); 96 | 97 | return RUN_ALL_TESTS(); 98 | } 99 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_succeed/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | sleep(1.0); 36 | 37 | // | ------------------------ set gains ----------------------- | 38 | 39 | const std::string desired_gains = "supersoft"; 40 | 41 | { 42 | 43 | auto [success, message] = uh->setGains(desired_gains); 44 | 45 | if (!success) { 46 | ROS_ERROR("[%s]: failed to switch gains to '%s', message: '%s'", ros::this_node::getName().c_str(), desired_gains.c_str(), message.c_str()); 47 | return false; 48 | } 49 | } 50 | 51 | sleep(1.0); 52 | 53 | // | ----------------------- check gains ---------------------- | 54 | 55 | if (uh->sh_gain_manager_diag_.getMsg()->current_name != desired_gains) { 56 | ROS_ERROR("[%s]: gains not set", ros::this_node::getName().c_str()); 57 | return false; 58 | } 59 | 60 | if (uh->isFlyingNormally()) { 61 | return true; 62 | } else { 63 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 64 | return false; 65 | } 66 | } 67 | 68 | 69 | TEST(TESTSuite, test) { 70 | 71 | Tester tester; 72 | 73 | bool result = tester.test(); 74 | 75 | if (result) { 76 | GTEST_SUCCEED(); 77 | } else { 78 | GTEST_FAIL(); 79 | } 80 | } 81 | 82 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 83 | 84 | ros::init(argc, argv, "test"); 85 | 86 | testing::InitGoogleTest(&argc, argv); 87 | 88 | return RUN_ALL_TESTS(); 89 | } 90 | -------------------------------------------------------------------------------- /test/control_manager/eland_control_error/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->gotoRel(2, 2, 2, 0); 37 | 38 | if (success) { 39 | ROS_ERROR("[%s]: goto succeeded, that should not happen in this case: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | // | -------------- wait for the eland to trigger ------------- | 45 | 46 | while (true) { 47 | 48 | if (!ros::ok()) { 49 | return false; 50 | } 51 | 52 | if (!uh->isFlyingNormally() && uh->getActiveController() == "EmergencyController" && uh->getActiveTracker() == "LandoffTracker") { 53 | break; 54 | } 55 | 56 | sleep(0.01); 57 | } 58 | 59 | // | -------------------- wait for landing -------------------- | 60 | 61 | while (true) { 62 | 63 | if (!ros::ok()) { 64 | return false; 65 | } 66 | 67 | if (!uh->isOutputEnabled()) { 68 | return true; 69 | } 70 | 71 | sleep(0.01); 72 | } 73 | 74 | return false; 75 | } 76 | 77 | 78 | TEST(TESTSuite, test) { 79 | 80 | Tester tester; 81 | 82 | bool result = tester.test(); 83 | 84 | if (result) { 85 | GTEST_SUCCEED(); 86 | } else { 87 | GTEST_FAIL(); 88 | } 89 | } 90 | 91 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 92 | 93 | ros::init(argc, argv, "test"); 94 | 95 | testing::InitGoogleTest(&argc, argv); 96 | 97 | return RUN_ALL_TESTS(); 98 | } 99 | -------------------------------------------------------------------------------- /config/private/control_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | control_manager: 4 | 5 | # list of names of dynamically loaded trackers 6 | mrs_trackers : [ 7 | "NullTracker", 8 | "MpcTracker", 9 | "LandoffTracker", 10 | "JoyTracker", 11 | "SpeedTracker", 12 | "MidairActivationTracker", 13 | ] 14 | 15 | # list of names of dynamically loaded controllers 16 | mrs_controllers : [ 17 | "Se3Controller", 18 | "MpcController", 19 | "FailsafeController", 20 | "EmergencyController", 21 | "MidairActivationController", 22 | ] 23 | 24 | #- this is the "do nothing tracker", which is active when no other is 25 | null_tracker: "NullTracker" 26 | 27 | status_timer_rate: 10 # [Hz] # will be overriden for VELOCITY+ output mode 28 | 29 | safety: 30 | 31 | safety_timer_rate: 100 # [Hz] 32 | failsafe_timer_rate: 100 # [Hz] 33 | 34 | # these constraints are normally overridden by the ConstraintManager. 35 | default_constraints: 36 | 37 | horizontal: 38 | speed: 1.0 # [m/s] 39 | acceleration: 1.0 # [m/s^2] 40 | jerk: 20.0 # [m/s^3] 41 | snap: 20.0 # [m/s^4] 42 | 43 | vertical: 44 | 45 | ascending: 46 | speed: 1.0 # [m/s] 47 | acceleration: 1.0 # [m/s^2] 48 | jerk: 20.0 # [m/s^3] 49 | snap: 20.0 # [m/s^4] 50 | 51 | descending: 52 | speed: 1.0 # [m/s] 53 | acceleration: 1.0 # [m/s^2] 54 | jerk: 20.0 # [m/s^3] 55 | snap: 20.0 # [m/s^4] 56 | 57 | heading: 58 | speed: 1.0 # [rad/s] 59 | acceleration: 1.0 # [rad/s^2] 60 | jerk: 10.0 # [rad/s^3] 61 | snap: 10.0 # [rad/s^4] 62 | 63 | angular_speed: 64 | roll: 20.0 # [rad/s] 65 | pitch: 20.0 # [rad/s] 66 | yaw: 10.0 # [rad/s] 67 | 68 | tilt: 60.0 # [deg] 69 | 70 | scope_timer: 71 | 72 | enabled: false 73 | log_filename: "" # if empty, scope timers output to terminal; otherwise log to file 74 | 75 | obstacle_bumper: 76 | 77 | timer_rate: 20.0 # [Hz] 78 | 79 | horizontal: 80 | overshoot: 0.2 # [m] 81 | 82 | vertical: 83 | overshoot: 0.2 # [m] 84 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_succeed/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | sleep(1.0); 36 | 37 | // | ------------------------ set constraints ----------------------- | 38 | 39 | const std::string desired_constraints = "medium"; 40 | 41 | { 42 | auto [success, message] = uh->setConstraints(desired_constraints); 43 | 44 | if (!success) { 45 | ROS_ERROR("[%s]: failed to switch constraints to '%s', message: '%s'", ros::this_node::getName().c_str(), desired_constraints.c_str(), message.c_str()); 46 | return false; 47 | } 48 | } 49 | 50 | sleep(1.0); 51 | 52 | // | ----------------------- check constraints ---------------------- | 53 | 54 | if (uh->sh_constraint_manager_diag_.getMsg()->current_name != desired_constraints) { 55 | ROS_ERROR("[%s]: constraints not set", ros::this_node::getName().c_str()); 56 | return false; 57 | } 58 | 59 | if (uh->isFlyingNormally()) { 60 | return true; 61 | } else { 62 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 63 | return false; 64 | } 65 | } 66 | 67 | 68 | TEST(TESTSuite, test) { 69 | 70 | Tester tester; 71 | 72 | bool result = tester.test(); 73 | 74 | if (result) { 75 | GTEST_SUCCEED(); 76 | } else { 77 | GTEST_FAIL(); 78 | } 79 | } 80 | 81 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 82 | 83 | ros::init(argc, argv, "test"); 84 | 85 | testing::InitGoogleTest(&argc, argv); 86 | 87 | return RUN_ALL_TESTS(); 88 | } 89 | -------------------------------------------------------------------------------- /test/control_manager/set_heading_relative_service/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | { 36 | auto [success, message] = uh->setHeadingRelative(0.5); 37 | 38 | if (!success) { 39 | ROS_ERROR("[%s]: set heading relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 40 | return false; 41 | } 42 | } 43 | 44 | { 45 | auto [success, message] = uh->setHeadingRelative(3.14); 46 | 47 | if (!success) { 48 | ROS_ERROR("[%s]: set heading relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 49 | return false; 50 | } 51 | } 52 | 53 | { 54 | auto [success, message] = uh->setHeadingRelative(0); 55 | 56 | if (!success) { 57 | ROS_ERROR("[%s]: set heading relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 58 | return false; 59 | } 60 | } 61 | 62 | this->sleep(5.0); 63 | 64 | if (uh->isFlyingNormally()) { 65 | return true; 66 | } else { 67 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 68 | return false; 69 | } 70 | } 71 | 72 | TEST(TESTSuite, test) { 73 | 74 | Tester tester; 75 | 76 | bool result = tester.test(); 77 | 78 | if (result) { 79 | GTEST_SUCCEED(); 80 | } else { 81 | GTEST_FAIL(); 82 | } 83 | } 84 | 85 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 86 | 87 | ros::init(argc, argv, "test"); 88 | 89 | testing::InitGoogleTest(&argc, argv); 90 | 91 | return RUN_ALL_TESTS(); 92 | } 93 | -------------------------------------------------------------------------------- /include/control_manager/output_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANAGER_OUTPUT_PUBLISHER 2 | #define CONTROL_MANAGER_OUTPUT_PUBLISHER 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace mrs_uav_managers 9 | { 10 | 11 | namespace control_manager 12 | { 13 | 14 | class OutputPublisher { 15 | 16 | public: 17 | OutputPublisher(); 18 | 19 | OutputPublisher(ros::NodeHandle& nh); 20 | 21 | void publish(const Controller::HwApiOutputVariant& control_output); 22 | 23 | private: 24 | mrs_lib::PublisherHandler ph_hw_api_actuator_cmd_; 25 | mrs_lib::PublisherHandler ph_hw_api_control_group_cmd_; 26 | mrs_lib::PublisherHandler ph_hw_api_attitude_rate_cmd_; 27 | mrs_lib::PublisherHandler ph_hw_api_attitude_cmd_; 28 | mrs_lib::PublisherHandler ph_hw_api_acceleration_hdg_rate_cmd_; 29 | mrs_lib::PublisherHandler ph_hw_api_acceleration_hdg_cmd_; 30 | mrs_lib::PublisherHandler ph_hw_api_velocity_hdg_rate_cmd_; 31 | mrs_lib::PublisherHandler ph_hw_api_velocity_hdg_cmd_; 32 | mrs_lib::PublisherHandler ph_hw_api_position_cmd_; 33 | 34 | void publish(const mrs_msgs::HwApiActuatorCmd& msg); 35 | void publish(const mrs_msgs::HwApiControlGroupCmd& msg); 36 | void publish(const mrs_msgs::HwApiAttitudeRateCmd& msg); 37 | void publish(const mrs_msgs::HwApiAttitudeCmd& msg); 38 | void publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg); 39 | void publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg); 40 | void publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg); 41 | void publish(const mrs_msgs::HwApiVelocityHdgCmd& msg); 42 | void publish(const mrs_msgs::HwApiPositionCmd& msg); 43 | 44 | class PublisherVisitor { 45 | 46 | public: 47 | PublisherVisitor(OutputPublisher* obj) : obj_(obj){}; 48 | 49 | OutputPublisher* obj_; 50 | 51 | template 52 | void operator()(const T& msg) { 53 | obj_->publish(msg); 54 | } 55 | }; 56 | }; 57 | 58 | } // namespace control_manager 59 | 60 | } // namespace mrs_uav_managers 61 | 62 | #endif // CONTROL_MANAGER_OUTPUT_PUBLISHER 63 | -------------------------------------------------------------------------------- /test/gain_manager/set_gains_should_fail/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | sleep(1.0); 36 | 37 | // | --------------- remember the current gains --------------- | 38 | 39 | const std::string old_gains = uh->sh_gain_manager_diag_.getMsg()->current_name; 40 | 41 | // | -------------------- try to set gains -------------------- | 42 | 43 | { 44 | const std::string gains = "do_not_exists"; 45 | 46 | auto [success, message] = uh->setGains(gains); 47 | 48 | if (success) { 49 | ROS_ERROR("[%s]: switching to gains '%s' secceeded, which should not happen", ros::this_node::getName().c_str(), gains.c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | sleep(1.0); 55 | 56 | // | ----------------------- check gains ---------------------- | 57 | 58 | if (uh->sh_gain_manager_diag_.getMsg()->current_name != old_gains) { 59 | ROS_ERROR("[%s]: gains changed even though they should not", ros::this_node::getName().c_str()); 60 | return false; 61 | } 62 | 63 | if (uh->isFlyingNormally()) { 64 | return true; 65 | } else { 66 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 67 | return false; 68 | } 69 | } 70 | 71 | 72 | TEST(TESTSuite, test) { 73 | 74 | Tester tester; 75 | 76 | bool result = tester.test(); 77 | 78 | if (result) { 79 | GTEST_SUCCEED(); 80 | } else { 81 | GTEST_FAIL(); 82 | } 83 | } 84 | 85 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 86 | 87 | ros::init(argc, argv, "test"); 88 | 89 | testing::InitGoogleTest(&argc, argv); 90 | 91 | return RUN_ALL_TESTS(); 92 | } 93 | -------------------------------------------------------------------------------- /config/public/uav_manager.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | uav_manager: 4 | 5 | # require gain or constraint manager running to takeoff? 6 | require_gain_manager: true 7 | require_constraint_manager: true 8 | 9 | # periodically checks if max height was exceeded 10 | # if so, retakes control and descends below it 11 | max_height_checking: 12 | 13 | enabled: true 14 | rate: 10.0 # [Hz] 15 | safety_height_offset: 0.25 # how much lower to descend below the max height 16 | 17 | # periodically checks if min height was exceeded 18 | # if so, retakes control and ascends above it 19 | min_height_checking: 20 | 21 | enabled: true 22 | rate: 10.0 # [Hz] 23 | min_height: 0.5 # [m] 24 | safety_height_offset: 0.25 # how much higher to ascend above the min height 25 | 26 | takeoff: 27 | 28 | rate: 10 # [Hz] 29 | 30 | during_takeoff: 31 | controller: "MpcController" 32 | tracker: "LandoffTracker" 33 | 34 | after_takeoff: 35 | controller: "MpcController" 36 | tracker: "MpcTracker" 37 | 38 | takeoff_height: 1.5 39 | 40 | landing: 41 | 42 | rate: 10 # [Hz] 43 | landing_tracker: "LandoffTracker" 44 | landing_controller: "MpcController" 45 | 46 | # those two must apply simultaneously 47 | landing_cutoff_mass_factor: 0.5 # how much lighter does the drone appear to be? 48 | landing_cutoff_timeout: 2.0 # [s] how long does the throttle has to be below the mass factor 49 | 50 | disarm: true 51 | 52 | # if the height estimate is available, and 53 | # if the UAV higher than this height: 54 | # the UAV will first descend to this height using a goto command, 55 | # and then land slowly using the landing tracker and controller 56 | descend_height: 2.0 # [m] 57 | 58 | tracking_tolerance: 59 | translation: 0.1 # [m] 60 | heading: 0.1 # [rad] 61 | 62 | midair_activation: 63 | 64 | after_activation: 65 | controller: "MpcController" 66 | tracker: "MpcTracker" 67 | 68 | flight_timer: 69 | 70 | enabled: false 71 | rate: 1 # [Hz] 72 | max_time: 10 # [s] 73 | 74 | # detecting if desired throttle cross max threshold, triggers landing after that 75 | max_throttle: 76 | 77 | enabled: true 78 | rate: 30 # [Hz] 79 | max_throttle: 0.80 # [-] 80 | eland_timeout: 1.0 # [s] 81 | ungrip_timeout: 0.5 # [s] 82 | -------------------------------------------------------------------------------- /test/uav_manager/max_height_check/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | // | --------------- goto to violate min height --------------- | 36 | 37 | { 38 | auto [success, message] = uh->gotoAbs(0, 0, 100, 0); 39 | 40 | if (success) { 41 | ROS_ERROR("[%s]: goto should fail", ros::this_node::getName().c_str()); 42 | return false; 43 | } 44 | } 45 | 46 | // | --------- wait till we are flying normally again --------- | 47 | 48 | while (true) { 49 | 50 | if (!ros::ok()) { 51 | return false; 52 | } 53 | 54 | if (uh->isFlyingNormally()) { 55 | break; 56 | } 57 | } 58 | 59 | // | ------------------- check the altitude ------------------- | 60 | 61 | sleep(1.0); 62 | 63 | if (!uh->sh_max_height_.hasMsg()) { 64 | ROS_ERROR("[%s]: missing max height msgs", ros::this_node::getName().c_str()); 65 | return true; 66 | } 67 | 68 | double max_height_agl = uh->sh_max_height_.getMsg()->value; 69 | 70 | auto height = uh->getHeightAgl(); 71 | 72 | if (height) { 73 | if (height.value() < max_height_agl) { 74 | return true; 75 | } 76 | } 77 | 78 | return false; 79 | } 80 | 81 | TEST(TESTSuite, test) { 82 | 83 | Tester tester; 84 | 85 | bool result = tester.test(); 86 | 87 | if (result) { 88 | GTEST_SUCCEED(); 89 | } else { 90 | GTEST_FAIL(); 91 | } 92 | } 93 | 94 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 95 | 96 | ros::init(argc, argv, "test"); 97 | 98 | testing::InitGoogleTest(&argc, argv); 99 | 100 | return RUN_ALL_TESTS(); 101 | } 102 | -------------------------------------------------------------------------------- /test/constraint_manager/set_constraints_should_fail/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | class Tester : public mrs_uav_testing::TestGeneric { 6 | 7 | public: 8 | bool test(); 9 | }; 10 | 11 | bool Tester::test() { 12 | 13 | std::shared_ptr uh; 14 | 15 | { 16 | auto [uhopt, message] = getUAVHandler(_uav_name_); 17 | 18 | if (!uhopt) { 19 | ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); 20 | return false; 21 | } 22 | 23 | uh = uhopt.value(); 24 | } 25 | 26 | { 27 | auto [success, message] = uh->activateMidAir(); 28 | 29 | if (!success) { 30 | ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); 31 | return false; 32 | } 33 | } 34 | 35 | sleep(1.0); 36 | 37 | // | --------------- remember the current constraints --------------- | 38 | 39 | const std::string old_constraints = uh->sh_constraint_manager_diag_.getMsg()->current_name; 40 | 41 | // | -------------------- try to set constraints -------------------- | 42 | 43 | { 44 | const std::string constraints = "do_not_exists"; 45 | 46 | auto [success, message] = uh->setConstraints(constraints); 47 | 48 | if (success) { 49 | ROS_ERROR("[%s]: switching to constraints '%s' secceeded, which should not happen", ros::this_node::getName().c_str(), constraints.c_str()); 50 | return false; 51 | } 52 | } 53 | 54 | sleep(1.0); 55 | 56 | // | ----------------------- check constraints ---------------------- | 57 | 58 | if (uh->sh_constraint_manager_diag_.getMsg()->current_name != old_constraints) { 59 | ROS_ERROR("[%s]: constraints changed even though they should not", ros::this_node::getName().c_str()); 60 | return false; 61 | } 62 | 63 | if (uh->isFlyingNormally()) { 64 | return true; 65 | } else { 66 | ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); 67 | return false; 68 | } 69 | } 70 | 71 | 72 | TEST(TESTSuite, test) { 73 | 74 | Tester tester; 75 | 76 | bool result = tester.test(); 77 | 78 | if (result) { 79 | GTEST_SUCCEED(); 80 | } else { 81 | GTEST_FAIL(); 82 | } 83 | } 84 | 85 | int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { 86 | 87 | ros::init(argc, argv, "test"); 88 | 89 | testing::InitGoogleTest(&argc, argv); 90 | 91 | return RUN_ALL_TESTS(); 92 | } 93 | --------------------------------------------------------------------------------