├── .gitignore ├── .vscode └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── installAshets.sh ├── launch ├── .vscode │ ├── c_cpp_properties.json │ └── settings.json ├── new_posix_sitl.launch ├── simplify_track.launch └── uav_launch.launch ├── models ├── aruco_marker │ ├── meshes │ │ ├── marker23.png │ │ ├── marker33.png │ │ └── model.dae │ ├── model.config │ └── model.sdf └── iris_fpv_down_cam.sdf ├── package.xml ├── scripts ├── 1018_iris_fpv_cam ├── ArucoNavigationServers.py ├── MarkerDetection.py ├── px4_launch_gazebo.sh └── target_pos_servers.py ├── src ├── SimplifyTrack.py └── simpleDrone.py ├── srv ├── goto_aruco.srv ├── land_aruco.srv ├── target_global_pos.srv └── target_local_pos.srv └── worlds └── aruco.world /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # vscode 132 | .vscode/* 133 | !.vscode/settings.json 134 | !.vscode/tasks.json 135 | !.vscode/launch.json 136 | !.vscode/extensions.json 137 | *.code-workspace -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/antony/catkin_ws/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ] 6 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uav_mavros_simulation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | sensor_msgs 14 | message_generation 15 | geographic_msgs 16 | mavros_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | target_global_pos.srv 63 | target_local_pos.srv 64 | goto_aruco.srv 65 | land_aruco.srv 66 | ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | generate_messages( 77 | DEPENDENCIES 78 | std_msgs # Or other packages containing msgs 79 | sensor_msgs 80 | mavros_msgs 81 | ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES uav_mavros_simulation 115 | # CATKIN_DEPENDS rospy 116 | # DEPENDS system_lib 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | # include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/uav_mavros_simulation.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/uav_mavros_simulation_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # catkin_install_python(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 176 | # install(TARGETS ${PROJECT_NAME}_node 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark libraries for installation 181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 182 | # install(TARGETS ${PROJECT_NAME} 183 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 186 | # ) 187 | 188 | ## Mark cpp header files for installation 189 | # install(DIRECTORY include/${PROJECT_NAME}/ 190 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 191 | # FILES_MATCHING PATTERN "*.h" 192 | # PATTERN ".svn" EXCLUDE 193 | # ) 194 | 195 | ## Mark other files for installation (e.g. launch and bag files, etc.) 196 | # install(FILES 197 | # # myfile1 198 | # # myfile2 199 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 200 | # ) 201 | 202 | ############# 203 | ## Testing ## 204 | ############# 205 | 206 | ## Add gtest based cpp test target and link libraries 207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_uav_mavros_simulation.cpp) 208 | # if(TARGET ${PROJECT_NAME}-test) 209 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 210 | # endif() 211 | 212 | ## Add folders to be run by python nosetests 213 | # catkin_add_nosetests(test) 214 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, antonikaras 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # uav mavros simulation 2 | 3 | ## Description 4 | 5 | This package was developed as part of the course Introduction to UAS technology at the University of Southern Denmark. The goal is to land the drone to an ArUCO marker. The drone is simulated using SITL and PX4. 6 | 7 | A video demonstation of the system is here : https://youtu.be/k1WqiE6d8TA 8 | 9 | The branch docker_env contains a docker environment, without the neeed to install ROS in your system, with instructions on how to run it. 10 | 11 | ## Dependencies 12 | 13 | * ROS, tested on ros-melodic 14 | * the mavros package v1.10.2, https://dev.px4.io/master/en/ros/mavros_installation.html 15 | * QGroundControl, http://qgroundcontrol.com/ 16 | * the simple_pid python package, ```pip install simple-pid``` 17 | * PX4 firmware, tested on version v1.10.2, ``` git clone https://github.com/px4/firmware``` 18 | 19 | ## Instructions 20 | 21 | * Clone the package your catkin_workspace/src, ``` git clone https://github.com/antonikaras/uav_mavros_simulation.git ``` 22 | * ```./installAshets.sh PX4_dir```, PX4_dir is the directory you cloned the PX4 firmware 23 | * Add the location of the px4 firmware to the file /launch/uav_launch.launch, firm_dir 24 | * catkin build/ catkin_make 25 | * source devel/setup.bash 26 | * ``` roslaunch uav_mavros_simulation uav_launch.launch world:=aruco vehicle:=iris_fpv_cam sdf:=iris_fpv_down_cam ``` 27 | * ``` rosrun uav_mavros_simulation simpleDrone.py ``` 28 | * run QGroundControl 29 | 30 | ## UAV commander instructions 31 | 32 | * exit, to close the swarm commander 33 | * goto x y z yaw, uav will go to the specified location location 34 | * examp1es : ```goto -46 15 13 1.5 ```, ```goto aruco ``` 35 | * land [x y z yaw] -> uav will land at its current position or at the optional x, y, z, yaw, or at the aruco marker 36 | * examples : ```land ```, ```land 1 5 5 1.2 ```, ```land aruco``` 37 | * return -> uav will return to its home position and land 38 | * example : ``` return ``` 39 | -------------------------------------------------------------------------------- /installAshets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | FIRMDIR=$1 4 | CWD=$PWD 5 | if [[ "$FIRMDIR" == "" ]]; then 6 | echo "You did not supply the Firmware folder." 7 | exit 8 | fi 9 | 10 | cd $FIRMDIR 11 | 12 | echo "installing launch files." 13 | cp $CWD/launch/new_posix_sitl.launch launch/ -v 14 | 15 | echo "installing Gazebo Models." 16 | cp -r $CWD/models/aruco_marker Tools/sitl_gazebo/models/ -v 17 | cp -r $CWD/models/iris_fpv_down_cam.sdf Tools/sitl_gazebo/models/iris_fpv_cam/ -v 18 | 19 | echo "installing worlds." 20 | cp $CWD/worlds/* Tools/sitl_gazebo/worlds/ -v 21 | 22 | echo "installing camera parameters" 23 | cp $CWD/scripts/1018_iris_fpv_cam ROMFS/px4fmu_common/init.d-posix/ -v -------------------------------------------------------------------------------- /launch/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/antony/catkin_ws/devel/include/**", 10 | "/opt/ros/melodic/include/**", 11 | "/home/antony/catkin_ws/src/mavros/libmavconn/include/**", 12 | "/home/antony/catkin_ws/src/mav_comm/mav_msgs/include/**", 13 | "/home/antony/catkin_ws/src/mav_comm/mav_planning_msgs/include/**", 14 | "/home/antony/catkin_ws/src/mavlink_lora_2.0/ros/mavlink_lora/include/**", 15 | "/home/antony/catkin_ws/src/mavros/mavros/include/**", 16 | "/home/antony/catkin_ws/src/mavros/mavros_msgs/include/**", 17 | "/home/antony/catkin_ws/src/rotors_simulator/rotors_control/include/**", 18 | "/home/antony/catkin_ws/src/rotors_simulator/rotors_gazebo_plugins/include/**", 19 | "/home/antony/catkin_ws/src/rotors_simulator/rotors_hil_interface/include/**", 20 | "/home/antony/catkin_ws/src/rotors_simulator/rotors_joy_interface/include/**", 21 | "/home/antony/catkin_ws/src/mavros/test_mavros/include/**", 22 | "/home/antony/catkin_ws/src/turtlebot/turtlebot3_simulations/turtlebot3_fake/include/**", 23 | "/home/antony/catkin_ws/src/turtlebot/turtlebot3_simulations/turtlebot3_gazebo/include/**", 24 | "/home/antony/catkin_ws/src/turtlebot/turtlebot3/turtlebot3_slam/include/**", 25 | "/usr/include/**" 26 | ], 27 | "name": "ROS" 28 | } 29 | ] 30 | } -------------------------------------------------------------------------------- /launch/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/antony/catkin_ws/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ] 6 | } -------------------------------------------------------------------------------- /launch/new_posix_sitl.launch: -------------------------------------------------------------------------------- 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /launch/simplify_track.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/uav_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /models/aruco_marker/meshes/marker23.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonikaras/uav_mavros_simulation/cf3752dc83dcd486d0704b696dd6ab7941b71e66/models/aruco_marker/meshes/marker23.png -------------------------------------------------------------------------------- /models/aruco_marker/meshes/marker33.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonikaras/uav_mavros_simulation/cf3752dc83dcd486d0704b696dd6ab7941b71e66/models/aruco_marker/meshes/marker33.png -------------------------------------------------------------------------------- /models/aruco_marker/meshes/model.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 2.82.7 commit date:2020-03-12, commit time:05:06, hash:375c7dc4caf4 7 | 8 | 2020-04-29T08:24:11 9 | 2020-04-29T08:24:11 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 39.59775 19 | 1.777778 20 | 0.1 21 | 100 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 0 29 | 10 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 1000 1000 1000 39 | 1 40 | 0 41 | 0.00111109 42 | 43 | 44 | 45 | 46 | 0 47 | 0 48 | 1 49 | 1 50 | 1 51 | 1 52 | 1 53 | 0 54 | 0 55 | 0 56 | 1000 57 | 29.99998 58 | 75 59 | 0.15 60 | 0 61 | 1 62 | 2 63 | 0.04999995 64 | 30.002 65 | 1 66 | 3 67 | 2880 68 | 3 69 | 1 70 | 1 71 | 0.1 72 | 0.1 73 | 1 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | marker23_png 84 | 85 | 86 | 87 | 88 | marker23_png-surface 89 | 90 | 91 | 92 | 93 | 94 | 0 0 0 1 95 | 96 | 97 | 98 | 99 | 100 | 1.45 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | marker23.png 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 1 1 1 1 1 -1 1 -1 1 1 -1 -1 -1 1 1 -1 1 -1 -1 -1 1 -1 -1 -1 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 0 0 1 0 -1 0 -1 0 0 0 0 -1 1 0 0 0 1 0 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 0.9998997 9.998e-5 1.00099e-4 0.9999001 9.998e-5 1.00338e-4 0.625 0.75 0.375 1 0.375 0.75 0.625 0 0.375 0.25 0.375 0 0.1735411 0.8189588 0.2214589 0.7710411 0.2214589 0.8189588 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.25 0.375 0.5 0.375 0.25 0.9998997 9.998e-5 0.9998998 0.9998999 1.00099e-4 0.9999001 0.625 0.75 0.625 1 0.375 1 0.625 0 0.625 0.25 0.375 0.25 0.1735411 0.8189588 0.1735411 0.7710411 0.2214589 0.7710411 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.25 0.625 0.5 0.375 0.5 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 |

4 0 0 2 0 1 0 0 2 2 1 3 7 1 4 3 1 5 6 2 6 5 2 7 7 2 8 1 3 9 7 3 10 5 3 11 0 4 12 3 4 13 1 4 14 4 5 15 1 5 16 5 5 17 4 0 18 6 0 19 2 0 20 2 1 21 6 1 22 7 1 23 6 2 24 4 2 25 5 2 26 1 3 27 3 3 28 7 3 29 0 4 30 2 4 31 3 4 32 4 5 33 0 5 34 1 5 35

157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 165 | 166 | 167 | 168 | -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 169 | 170 | 171 | 172 | 1 0 0 0 0 1 0 0 0 0 0.01 0 0 0 0 1 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | -------------------------------------------------------------------------------- /models/aruco_marker/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Aruco marker 5 | 1.0 6 | 7 | model.sdf 8 | 9 | 10 | Jes Jepsen 11 | jegj@mmmi.sdu.dk 12 | 13 | 14 | 15 | Aruco marker for detection 16 | 17 | 18 | -------------------------------------------------------------------------------- /models/aruco_marker/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0 0 0 0 0 0 6 | 7 | 8 | 9 | 0 0 0 0 0 0 10 | 11 | 12 | 2 2 0.01 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | model://aruco_marker/meshes/model.dae 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /models/iris_fpv_down_cam.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 0 0 0 0 -0 0 8 | 1.5 9 | 10 | 0.029125 11 | 0 12 | 0 13 | 0.029125 14 | 0 15 | 0.055225 16 | 17 | 18 | 19 | 0 0 0 0 -0 0 20 | 21 | 22 | 0.47 0.47 0.11 23 | 24 | 25 | 26 | 27 | 28 | 0.001 29 | 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 0 0 0 0 -0 0 39 | 40 | 41 | 1 1 1 42 | model://rotors_description/meshes/iris.stl 43 | 44 | 45 | 46 | 50 | 51 | 52 | 1 53 | 54 | 55 | 56 | 0 0 0 0 -0 0 57 | 58 | 0 0 0 0 -0 0 59 | 0.015 60 | 61 | 1e-05 62 | 0 63 | 0 64 | 1e-05 65 | 0 66 | 1e-05 67 | 68 | 69 | 70 | 71 | /imu_link 72 | base_link 73 | 74 | 1 0 0 75 | 76 | 0 77 | 0 78 | 0 79 | 0 80 | 81 | 82 | 0 83 | 0 84 | 85 | 1 86 | 87 | 88 | 89 | 0.13 -0.22 0.023 0 -0 0 90 | 91 | 0 0 0 0 -0 0 92 | 0.005 93 | 94 | 9.75e-07 95 | 0 96 | 0 97 | 0.000273104 98 | 0 99 | 0.000274004 100 | 101 | 102 | 103 | 0 0 0 0 -0 0 104 | 105 | 106 | 0.005 107 | 0.128 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 0 0 0 0 -0 0 121 | 122 | 123 | 1 1 1 124 | model://rotors_description/meshes/iris_prop_ccw.dae 125 | 126 | 127 | 128 | 132 | 133 | 134 | 1 135 | 136 | 137 | 138 | rotor_0 139 | base_link 140 | 141 | 0 0 1 142 | 143 | -1e+16 144 | 1e+16 145 | 146 | 147 | 0 148 | 0 149 | 150 | 1 151 | 152 | 153 | 154 | -0.13 0.2 0.023 0 -0 0 155 | 156 | 0 0 0 0 -0 0 157 | 0.005 158 | 159 | 9.75e-07 160 | 0 161 | 0 162 | 0.000273104 163 | 0 164 | 0.000274004 165 | 166 | 167 | 168 | 0 0 0 0 -0 0 169 | 170 | 171 | 0.005 172 | 0.128 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 0 0 0 0 -0 0 186 | 187 | 188 | 1 1 1 189 | model://rotors_description/meshes/iris_prop_ccw.dae 190 | 191 | 192 | 193 | 197 | 198 | 199 | 1 200 | 201 | 202 | 203 | rotor_1 204 | base_link 205 | 206 | 0 0 1 207 | 208 | -1e+16 209 | 1e+16 210 | 211 | 212 | 0 213 | 0 214 | 215 | 1 216 | 217 | 218 | 219 | 0.13 0.22 0.023 0 -0 0 220 | 221 | 0 0 0 0 -0 0 222 | 0.005 223 | 224 | 9.75e-07 225 | 0 226 | 0 227 | 0.000273104 228 | 0 229 | 0.000274004 230 | 231 | 232 | 233 | 0 0 0 0 -0 0 234 | 235 | 236 | 0.005 237 | 0.128 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 0 0 0 0 -0 0 251 | 252 | 253 | 1 1 1 254 | model://rotors_description/meshes/iris_prop_cw.dae 255 | 256 | 257 | 258 | 262 | 263 | 264 | 1 265 | 266 | 267 | 268 | rotor_2 269 | base_link 270 | 271 | 0 0 1 272 | 273 | -1e+16 274 | 1e+16 275 | 276 | 277 | 0 278 | 0 279 | 280 | 1 281 | 282 | 283 | 284 | -0.13 -0.2 0.023 0 -0 0 285 | 286 | 0 0 0 0 -0 0 287 | 0.005 288 | 289 | 9.75e-07 290 | 0 291 | 0 292 | 0.000273104 293 | 0 294 | 0.000274004 295 | 296 | 297 | 298 | 0 0 0 0 -0 0 299 | 300 | 301 | 0.005 302 | 0.128 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 0 0 0 0 -0 0 316 | 317 | 318 | 1 1 1 319 | model://rotors_description/meshes/iris_prop_cw.dae 320 | 321 | 322 | 323 | 327 | 328 | 329 | 1 330 | 331 | 332 | 333 | rotor_3 334 | base_link 335 | 336 | 0 0 1 337 | 338 | -1e+16 339 | 1e+16 340 | 341 | 342 | 0 343 | 0 344 | 345 | 1 346 | 347 | 348 | 349 | 350 | base_link 351 | 10 352 | 353 | 354 | 355 | rotor_0_joint 356 | rotor_0 357 | ccw 358 | 0.0125 359 | 0.025 360 | 1100 361 | 8.54858e-06 362 | 0.06 363 | /gazebo/command/motor_speed 364 | 0 365 | 0.000175 366 | 1e-06 367 | /motor_speed/0 368 | 10 369 | 370 | 371 | 372 | rotor_1_joint 373 | rotor_1 374 | ccw 375 | 0.0125 376 | 0.025 377 | 1100 378 | 8.54858e-06 379 | 0.06 380 | /gazebo/command/motor_speed 381 | 1 382 | 0.000175 383 | 1e-06 384 | /motor_speed/1 385 | 10 386 | 387 | 388 | 389 | rotor_2_joint 390 | rotor_2 391 | cw 392 | 0.0125 393 | 0.025 394 | 1100 395 | 8.54858e-06 396 | 0.06 397 | /gazebo/command/motor_speed 398 | 2 399 | 0.000175 400 | 1e-06 401 | /motor_speed/2 402 | 10 403 | 404 | 405 | 406 | rotor_3_joint 407 | rotor_3 408 | cw 409 | 0.0125 410 | 0.025 411 | 1100 412 | 8.54858e-06 413 | 0.06 414 | /gazebo/command/motor_speed 415 | 3 416 | 0.000175 417 | 1e-06 418 | /motor_speed/3 419 | 10 420 | 421 | 422 | 423 | 1 424 | 425 | 426 | 427 | 100 428 | 0.0004 429 | 6.4e-06 430 | 600 431 | /mag 432 | 433 | 434 | 435 | 50 436 | /baro 437 | 438 | 439 | 440 | /gazebo/command/motor_speed 441 | 4 442 | 443 | 444 | 445 | /imu 446 | /gps 447 | /mag 448 | /baro 449 | INADDR_ANY 450 | 14560 451 | 4560 452 | 0 453 | /dev/ttyACM0 454 | 921600 455 | INADDR_ANY 456 | 14550 457 | INADDR_ANY 458 | 14540 459 | 0 460 | 0 461 | 0 462 | 1 463 | 0 464 | 0 465 | 1 466 | /gazebo/command/motor_speed 467 | 468 | 469 | 0 470 | 0 471 | 1000 472 | 0 473 | 100 474 | velocity 475 | 476 | 477 | 1 478 | 0 479 | 1000 480 | 0 481 | 100 482 | velocity 483 | 484 | 485 | 2 486 | 0 487 | 1000 488 | 0 489 | 100 490 | velocity 491 | 492 | 493 | 3 494 | 0 495 | 1000 496 | 0 497 | 100 498 | velocity 499 | 500 | 501 | 4 502 | 0 503 | 1000 504 | 0 505 | 100 506 | velocity 507 | 508 | 509 | 510 | 0 511 | 512 | 513 | /imu_link 514 | /imu 515 | 0.00018665 516 | 3.8785e-05 517 | 1000.0 518 | 0.0087 519 | 0.00186 520 | 0.006 521 | 300.0 522 | 0.196 523 | 524 | 525 | 526 | model://fpv_cam 527 | 0 0 0 0 1.57 0 528 | 529 | 530 | 531 | fpv_cam::link 532 | base_link 533 | 534 | 0 0 1 535 | 536 | 0 537 | 0 538 | 539 | 540 | 541 | 542 | 543 | 544 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uav_mavros_simulation 4 | 0.0.0 5 | The uav_mavros_simulation package 6 | 7 | 8 | 9 | 10 | antony 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | message_generation 56 | message_runtime 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /scripts/1018_iris_fpv_cam: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | sh /etc/init.d-posix/10016_iris 3 | param set NAV_DLL_ACT 0 4 | param set NAV_RCL_ACT 0 5 | param set COM_OBL_ACT 0 6 | -------------------------------------------------------------------------------- /scripts/ArucoNavigationServers.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python2 2 | 3 | import rospy 4 | import mavros 5 | import cv2 as cv 6 | import numpy as np 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge, CvBridgeError 9 | from nav_msgs.msg import Odometry 10 | from mavros_msgs.msg import PositionTarget as PT 11 | from geometry_msgs.msg import Vector3, TwistStamped 12 | from uav_mavros_simulation.srv import goto_aruco, goto_arucoResponse, land_aruco, land_arucoResponse 13 | from std_msgs.msg import Float32 14 | from tf import transformations as tr 15 | import mavros_msgs.msg 16 | import mavros.command 17 | from mavros import setpoint as SP 18 | from simple_pid import PID 19 | 20 | class ArucoNavigationController(): 21 | 22 | def __init__(self): 23 | ''' Class that acts as a server for the goto_aruco service and the land_aruco service ''' 24 | 25 | # init node 26 | rospy.init_node('aruco_navigation') 27 | mavros.set_namespace('mavros') 28 | 29 | # Setup Subscribers 30 | ## Marker pos 31 | aruco_pos = rospy.Subscriber('/aruco_marker_pos', PT, self._arucoCallback) 32 | ## mavros state 33 | state_sub = rospy.Subscriber(mavros.get_topic('state'), 34 | mavros_msgs.msg.State, self._state_callback) 35 | 36 | # Setup publishers 37 | # /mavros/setpoint_velocity/cmd_vel 38 | self.cmd_vel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) 39 | 40 | # setup services 41 | # /mavros/cmd/arming 42 | self.set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) 43 | # /mavros/set_mode 44 | self.set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) 45 | 46 | # Initialize the service servers 47 | goto_aruco_serv = rospy.Service('goto_aruco', goto_aruco, self.GotoAruco) 48 | land_aruco_serv = rospy.Service('land_aruco', land_aruco, self.LandAruco) 49 | 50 | # Initialize variables 51 | self.pos = [0.0] * 4 52 | self.markerPos = [0.0] * 4 53 | self.UAV_state = mavros_msgs.msg.State() 54 | self.markerHeight = 20.0 # Height above the marker 55 | 56 | # Setup rate 57 | self.rate = rospy.Rate(30) 58 | rospy.sleep(1) 59 | rospy.spin() 60 | 61 | def _arucoCallback(self, msg): 62 | ''' Callback for the aruco marker POS ''' 63 | self.markerPos[0] = msg.position.x 64 | self.markerPos[1] = msg.position.y 65 | self.markerPos[2] = msg.position.z 66 | self.markerPos[3] = msg.yaw 67 | 68 | self.markerPos = np.array(self.markerPos) 69 | 70 | def _state_callback(self, topic): 71 | self.UAV_state.armed = topic.armed 72 | self.UAV_state.connected = topic.connected 73 | self.UAV_state.mode = topic.mode 74 | self.UAV_state.guided = topic.guided 75 | 76 | def GotoAruco(self, req): 77 | ''' Goto the aruco marker ''' 78 | rospy.loginfo('Going to aruco marker') 79 | timeOut = req.timeOut 80 | 81 | new_sp = TwistStamped() 82 | while self.UAV_state.mode != "OFFBOARD" : 83 | rospy.sleep(0.1) 84 | self.set_mode(0, 'OFFBOARD') 85 | # Publish something to activate the offboard mode 86 | self.cmd_vel_pub.publish(new_sp) 87 | 88 | if not mavros.command.arming(True) : 89 | mavros.command.arming(True) 90 | 91 | ts = rospy.Time.now() 92 | 93 | xPID = PID(.25, 0.05, 0.01, output_limits=(-.5, 0.5), setpoint=0.0) 94 | yPID = PID(.25, 0.05, 0.01, output_limits=(-.5, 0.5), setpoint=0.0) 95 | zPID = PID(.2, 0.0, 0.05, output_limits=(-0.5, 0.5), setpoint=self.markerHeight) 96 | yawPID = PID(.2, 0.0, 0.0, output_limits=(-1.0, 1.0), setpoint=0.0) 97 | 98 | while np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, -self.markerHeight])) > 1.0 and \ 99 | (rospy.Time.now() - ts < rospy.Duration(timeOut)): 100 | 101 | new_sp = TwistStamped() 102 | new_sp.twist.linear.x = xPID(-self.markerPos[0]) 103 | new_sp.twist.linear.y = yPID(-self.markerPos[1]) 104 | new_sp.twist.linear.z = zPID(-self.markerPos[2]) 105 | new_sp.twist.angular.z = yawPID(self.markerPos[3]) 106 | 107 | #print(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, -self.markerHeight]))) 108 | 109 | self.cmd_vel_pub.publish(new_sp) 110 | 111 | return goto_arucoResponse(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, -self.markerHeight]))) 112 | 113 | def LandAruco(self, req): 114 | ''' Land on the aruco marker ''' 115 | 116 | rospy.loginfo('Landing on the aruco marker') 117 | timeOut = req.timeOut 118 | 119 | new_sp = TwistStamped() 120 | while self.UAV_state.mode != "OFFBOARD" : 121 | rospy.sleep(0.1) 122 | self.set_mode(0, 'OFFBOARD') 123 | # Publish something to activate the offboard mode 124 | self.cmd_vel_pub.publish(new_sp) 125 | 126 | if not mavros.command.arming(True) : 127 | mavros.command.arming(True) 128 | 129 | ts = rospy.Time.now() 130 | 131 | xPID = PID(.4, 0.075, 0.02, output_limits=(-.5, 0.5), setpoint=0.0) 132 | yPID = PID(.4, 0.075, 0.02, output_limits=(-.5, 0.5), setpoint=0.0) 133 | zPID = PID(.5, 0.0, 0.05, output_limits=(-0.75, 0.75), setpoint=0.0) 134 | yawPID = PID(.2, 0.0, 0.0, output_limits=(-1.0, 1.0), setpoint=0.0) 135 | 136 | prev_height = self.markerPos[2] 137 | ts2 = rospy.Time.now() 138 | 139 | while (rospy.Time.now() - ts < rospy.Duration(timeOut)): 140 | 141 | new_sp = TwistStamped() 142 | new_sp.twist.linear.x = xPID(-self.markerPos[0]) 143 | new_sp.twist.linear.y = yPID(-self.markerPos[1]) 144 | new_sp.twist.linear.z = zPID(-self.markerPos[2]) 145 | new_sp.twist.angular.z = yawPID(self.markerPos[3]) 146 | 147 | if self.markerPos[2] != prev_height: 148 | ts2 = rospy.Time.now() 149 | prev_height = self.markerPos[2] 150 | 151 | if rospy.Time.now() - ts2 > rospy.Duration(1.0): 152 | break 153 | #print(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, -self.markerHeight]))) 154 | 155 | self.cmd_vel_pub.publish(new_sp) 156 | return land_arucoResponse(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, 0.0]))) 157 | 158 | ################################################################################################### 159 | if __name__ == "__main__": 160 | ANC = ArucoNavigationController() 161 | 162 | -------------------------------------------------------------------------------- /scripts/MarkerDetection.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python2 2 | 3 | import rospy 4 | import mavros 5 | import cv2 as cv 6 | import numpy as np 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge, CvBridgeError 9 | from nav_msgs.msg import Odometry 10 | from mavros_msgs.msg import PositionTarget as PT 11 | from std_msgs.msg import Float32 12 | from tf import transformations as tr 13 | import mavros_msgs.msg 14 | from mavros import setpoint as SP 15 | from simple_pid import PID 16 | 17 | class MarkerDetector: 18 | """ 19 | Subscribe to the camera feed and detect the aruco marker in the scene 20 | """ 21 | def __init__(self): 22 | 23 | # Init node 24 | rospy.init_node('marker_detector') 25 | mavros.set_namespace('mavros') 26 | 27 | # Setup subscribers 28 | ## Image 29 | video_topic = "iris_fpv_cam/usb_cam/image_raw" 30 | image_subscriber = rospy.Subscriber(video_topic, Image, self.image_callback) 31 | self.bridge = CvBridge() 32 | 33 | ## Drone pose 34 | # /mavros/local_position/pose 35 | local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), 36 | SP.PoseStamped, self._local_position_callback) 37 | 38 | # Set up publishers 39 | self.aruco_marker_pos_pub = rospy.Publisher('/aruco_marker_pos', PT, queue_size=10) 40 | self.aruco_marker_img_pub = rospy.Publisher('/aruco_marker_img', Image, queue_size=10) 41 | 42 | # Initialize variables 43 | self.frame = np.zeros((240, 320, 3), np.uint8) 44 | self.pos = [0.0] * 4 45 | self.markerPos = [0.0] * 4 46 | 47 | ## camera intristic parameters 48 | self.K = np.array([277.191356, 0.0, 160.5, 0.0, 277.191356, 120.5, 0.0, 0.0, 1.0]).reshape(3,3) 49 | self.distCoeffs = np.array([0.0] * 5) 50 | 51 | #Load the dictionary that was used to generate the markers. 52 | self.dictionary = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) 53 | # Initialize the detector parameters using default values 54 | self.parameters = cv.aruco.DetectorParameters_create() 55 | 56 | # Setup rate 57 | self.rate = rospy.Rate(30) 58 | self.rate.sleep() 59 | 60 | def _local_position_callback(self, topic): 61 | # Position data 62 | self.pos[0] = topic.pose.position.x 63 | self.pos[1] = topic.pose.position.y 64 | self.pos[2] = topic.pose.position.z 65 | 66 | # Orientation data 67 | (r, p, y) = tr.euler_from_quaternion([topic.pose.orientation.x, topic.pose.orientation.y, topic.pose.orientation.z, topic.pose.orientation.w]) 68 | self.pos[3] = y 69 | 70 | def image_callback(self, data): 71 | """ 72 | Save the image data everytime a new frame comes up 73 | """ 74 | try: 75 | self.frame = self.bridge.imgmsg_to_cv2(data, "bgr8") 76 | except CvBridgeError as e: 77 | print(e) 78 | 79 | def Aruco_marker_detector(self): 80 | """ Use the build in python library to detect the aruco marker and its coordinates """ 81 | img = self.frame.copy() 82 | 83 | # Detect the markers in the image 84 | markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters) 85 | # Get the transformation matrix to the marker 86 | if markerCorners: 87 | 88 | markerSize = 2.0 89 | axisLength = 3.0 90 | 91 | rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers( markerCorners, markerSize, self.K, self.distCoeffs) 92 | 93 | # Draw the axis on the marker 94 | frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength) 95 | 96 | rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0) 97 | rotMat = rotMat[0:3, 0:3] 98 | tvecs = np.matmul(rotMat, tvecs[0][0].T) 99 | rotMat, _ = cv.Rodrigues(rvecs) 100 | eul = tr.euler_from_matrix(rotMat) 101 | 102 | yaw_angle = self.pos[3] - eul[2] 103 | 104 | # Publish the position of the marker 105 | marker_pos = PT() 106 | marker_pos.position.x = tvecs[0] 107 | marker_pos.position.y = - tvecs[2] 108 | marker_pos.position.z = tvecs[1] 109 | marker_pos.yaw = eul[2] * np.pi / 180.0 110 | self.aruco_marker_pos_pub.publish(marker_pos) 111 | 112 | # Publish the image with the detected marker 113 | self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8")) 114 | 115 | ################################################################################################### 116 | if __name__ == "__main__": 117 | MD = MarkerDetector() 118 | while not rospy.is_shutdown(): 119 | MD.Aruco_marker_detector() 120 | 121 | 122 | -------------------------------------------------------------------------------- /scripts/px4_launch_gazebo.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # Export the desired gps coordinates 4 | export PX4_HOME_LAT=55.47197622 5 | export PX4_HOME_LON=10.32480953 6 | export PX4_HOME_ALT=7.4000000 7 | 8 | # Source the PX4 Firmware directory 9 | if [ -z "$1" ]; then 10 | # change this to a location of your firmware! 11 | FIRMDIR="/home/antony/Documents/Git/Firmware" 12 | else 13 | FIRMDIR=$1 14 | fi 15 | echo $FIRMDIR 16 | 17 | # source ros - remeber to change to your version, e.g. melodic/kinetic 18 | source /opt/ros/melodic/setup.bash 19 | # argument used to browse to your PX4 SITL firmware folder 20 | cd $FIRMDIR 21 | 22 | # Needed environment for running SITL 23 | source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default 24 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) 25 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo 26 | 27 | # Run basic PX4 SITL 28 | echo $world 29 | roslaunch px4 new_posix_sitl.launch world:=$2 vehicle:=$3 sdf:=$4 30 | -------------------------------------------------------------------------------- /scripts/target_pos_servers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # import ROS libraries 4 | import rospy 5 | import mavros 6 | from mavros.utils import * 7 | import mavros.setpoint 8 | import mavros.command 9 | import mavros_msgs.msg 10 | import mavros_msgs.srv 11 | from sensor_msgs.msg import NavSatFix 12 | from uav_mavros_simulation.srv import target_global_pos, target_global_posResponse, target_local_pos, target_local_posResponse 13 | from mavros import setpoint as SP 14 | from mavros_msgs.msg import GlobalPositionTarget as GPT 15 | import sys 16 | import signal 17 | from geometry_msgs.msg import Vector3, TwistStamped 18 | from std_msgs.msg import Float32 19 | import math 20 | import numpy as np 21 | import utm 22 | import tf 23 | 24 | class MoveToPos(): 25 | """ 26 | Class that is used to create a servers for the services that move 27 | the drone to a specific position in the global and local coordinate system 28 | """ 29 | def __init__(self): 30 | 31 | # Setup the nodes and the namespace 32 | # If the drone is a slave generate the master ID 33 | 34 | rospy.init_node("uav_pos_services", anonymous=True) 35 | 36 | self.rate = rospy.Rate(20) 37 | mavros.set_namespace('mavros') 38 | 39 | # Initialize the parameters 40 | self.local_pos = [0.0] * 4 41 | self.global_pos = [0.0] * 4 42 | self.UAV_state = mavros_msgs.msg.State() 43 | 44 | # setup subscribers 45 | # /mavros/state 46 | state_sub = rospy.Subscriber(mavros.get_topic('state'), 47 | mavros_msgs.msg.State, self._state_callback) 48 | 49 | # /mavros/local_position/pose 50 | local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), 51 | SP.PoseStamped, self._local_position_callback) 52 | # /mavros/global_position/global 53 | global_position_sub = rospy.Subscriber('mavros/global_position/global', 54 | NavSatFix, self._global_position_callback) 55 | 56 | # setup publisher 57 | # /mavros/setpoint/position/local 58 | self.setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) 59 | 60 | # setup the services 61 | # /mavros/cmd/arming 62 | self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) 63 | # /mavros/set_mode 64 | self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 65 | 66 | # Setup global and local service server 67 | sg = rospy.Service("target_global_pos", target_global_pos, self.GotoGlobPos) 68 | sl = rospy.Service("target_local_pos", target_local_pos, self.GotoLocPos) 69 | 70 | self.setpoint_msg = mavros.setpoint.PoseStamped( 71 | header=mavros.setpoint.Header( 72 | frame_id="att_pose", 73 | stamp=rospy.Time.now()), 74 | ) 75 | 76 | rospy.loginfo("The target services were successfully initiated") 77 | 78 | rospy.spin() 79 | 80 | def _state_callback(self, topic): 81 | self.UAV_state.armed = topic.armed 82 | self.UAV_state.connected = topic.connected 83 | self.UAV_state.mode = topic.mode 84 | self.UAV_state.guided = topic.guided 85 | 86 | def _local_position_callback(self, topic): 87 | # Position data 88 | self.local_pos[0] = topic.pose.position.x 89 | self.local_pos[1] = topic.pose.position.y 90 | self.local_pos[2] = topic.pose.position.z 91 | 92 | # Orientation data 93 | (r, p, y) = tf.transformations.euler_from_quaternion([topic.pose.orientation.x, topic.pose.orientation.y, topic.pose.orientation.z, topic.pose.orientation.w]) 94 | self.local_pos[3] = y 95 | self.global_pos[3] = y 96 | 97 | def _global_position_callback(self, data): 98 | self.global_pos[0] = data.latitude 99 | self.global_pos[1] = data.longitude 100 | self.global_pos[2] = data.altitude 101 | 102 | def GotoLocalPos(self, x, y, z, yaw): 103 | """ Enter the desired x, y, z coordinates and the yaw angle in the local system """ 104 | 105 | dist = np.linalg.norm(np.array(self.local_pos[0:3]) - np.array([x, y, z])) 106 | rospy.loginfo("estimated distance : " + str(dist)) 107 | 108 | self.set_mode(0, 'OFFBOARD') 109 | if (not self.UAV_state.armed): 110 | mavros.command.arming(True) 111 | 112 | last_request = rospy.Time.now() 113 | t0 = last_request 114 | 115 | # enter the main loop 116 | while ((np.linalg.norm(np.array(self.local_pos[0:3]) - np.array([x, y, z])) > 0.1) or 117 | (abs(yaw - self.local_pos[3]) > 0.01)): 118 | # print "Entered whiled loop" 119 | if (self.UAV_state.mode != "OFFBOARD" and 120 | (rospy.Time.now() - last_request > rospy.Duration(5.0))): 121 | self.set_mode(0, 'OFFBOARD') 122 | print("uav enabling offboard mode") 123 | last_request = rospy.Time.now() 124 | else: 125 | if (not self.UAV_state.armed and 126 | (rospy.Time.now() - last_request > rospy.Duration(5.0))): 127 | if (mavros.command.arming(True)): 128 | print("uav armed") 129 | last_request = rospy.Time.now() 130 | 131 | # Position 132 | self.setpoint_msg.pose.position.x = x 133 | self.setpoint_msg.pose.position.y = y 134 | self.setpoint_msg.pose.position.z = z 135 | 136 | # Orientation 137 | (qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(0, 0, yaw) 138 | self.setpoint_msg.pose.orientation.x = qx 139 | self.setpoint_msg.pose.orientation.y = qy 140 | self.setpoint_msg.pose.orientation.z = qz 141 | self.setpoint_msg.pose.orientation.w = qw 142 | 143 | #print("Height: %f" % self.setpoint_msg.pose.position.z) 144 | self.setpoint_local_pub.publish(self.setpoint_msg) 145 | #print(np.linalg.norm(np.array(self.local_pos) - np.array([x, y, z]))) 146 | self.rate.sleep() 147 | 148 | # Start hovering 149 | #self.hover_pub.publish(self.local_pos[2]) 150 | 151 | return np.linalg.norm(np.array(self.local_pos[0:3]) - np.array([x, y, z])) 152 | 153 | def GotoLocPos(self, req): 154 | 155 | x = req.goal_pos.position.x 156 | y = req.goal_pos.position.y 157 | z = req.goal_pos.position.z 158 | Y = req.goal_pos.yaw 159 | 160 | rospy.loginfo("uav received local target") 161 | 162 | dist = self.GotoLocalPos(x, y, z, Y) 163 | 164 | return target_local_posResponse(dist) 165 | 166 | def GotoGlobPos(self, req): 167 | """ Enter the desired latitude, longitude, and altitude for the drone to go """ 168 | 169 | lat = req.goal_pos.latitude 170 | lon = req.goal_pos.longitude 171 | alt = req.goal_pos.altitude 172 | yaw = req.goal_pos.yaw 173 | # Convert the global position to the local 174 | x1, y1, z1, u = utm.from_latlon(self.global_pos[0], self.global_pos[1]) 175 | x2, y2, z2, u = utm.from_latlon(lat, lon) 176 | 177 | rospy.loginfo("uav received global target") 178 | 179 | dist = self.GotoLocalPos(x2 - x1, y2 - y1, alt - self.global_pos[2], yaw) 180 | 181 | return target_global_posResponse(dist) 182 | 183 | ################################################################################################### 184 | if __name__ == "__main__": 185 | MTP = MoveToPos() 186 | -------------------------------------------------------------------------------- /src/SimplifyTrack.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python2 2 | 3 | # ROS libraries 4 | import rospy 5 | import rosbag 6 | from mavlink_msgs.msg import mavlink_lora_pos as ml_pos 7 | import numpy as np 8 | import utm 9 | import sys 10 | import matplotlib.pyplot as plt 11 | 12 | 13 | class SimplifyTrack(): 14 | 15 | def __init__(self, bag): 16 | ''' 17 | Read the gnss data from the rosbag, convert them to UTM, remove the outliers, 18 | simplify the track, convert them back to gnss and export them as a route plan 19 | for the uav 20 | ''' 21 | # Initialize the node 22 | rospy.init_node('simplify_track') 23 | 24 | # Initialize position data 25 | self.gnss_data = [] 26 | self.utm_data = [] 27 | self.outliers = 0 28 | 29 | # Initialize the speed data 30 | self.Speed = [] 31 | self.avgSpeed = 0 32 | self.varSpeed = 0 33 | 34 | # Play the rosbag and save the position data 35 | self.PlayBag(bag) 36 | 37 | # Remove the outliers 38 | self.SpeedComputation() 39 | print(self.avgSpeed, self.varSpeed) 40 | self.RemoveOutliers() 41 | 42 | self.PlotData(self.utm_data) 43 | 44 | 45 | def PlayBag(self, bag): 46 | ''' Play the rosbag @bag''' 47 | 48 | Bag = rosbag.Bag(bag) 49 | for topic, msg, t in Bag.read_messages(topics=['/mavlink_pos']): 50 | # Read the gnss data 51 | self.gnss_data.append([msg.lat, msg.lon, msg.alt, msg.relative_alt, msg.time_usec / 1.0e6]) 52 | 53 | # Convert the gnss data to UTM 54 | x1, y1, z1, u = utm.from_latlon(msg.lat, msg.lon) 55 | self.utm_data.append([x1, y1, z1, u, msg.time_usec / 1.0e6]) 56 | 57 | ##### Outlier removal ##### 58 | def SpeedComputation(self): 59 | ''' Compute the speed using the utm coordinates ''' 60 | # Initialize the speed data 61 | self.Speed = [] 62 | self.avgSpeed = 0 63 | self.varSpeed = 0 64 | 65 | for i in range(1, len(self.utm_data)): 66 | 67 | utm_cur = np.array(self.utm_data[i][0:2]) 68 | utm_prev = np.array(self.utm_data[i - 1][0:2]) 69 | 70 | dt = self.utm_data[i][4] - self.utm_data[i - 1][4] 71 | dx = utm_cur[0] - utm_prev[0] 72 | dy = utm_cur[1] - utm_prev[1] 73 | 74 | sp = 0 75 | if (dt > 0): 76 | sp = np.sqrt(dx * dx + dy * dy) / dt 77 | #print(sp, dt, dx, dy) 78 | self.Speed.append(sp) 79 | 80 | self.avgSpeed = np.mean(self.Speed) 81 | self.varSpeed = np.var(self.Speed) 82 | 83 | # Remove the outliers 84 | def RemoveOutliers(self): 85 | ''' Remove the outliers ''' 86 | # Compute the mean and variance of the speed to remove outliers 87 | self.SpeedComputation() 88 | 89 | # Save the length before 90 | ln_bef = len(self.utm_data) 91 | i = 1 92 | while (i < len(self.utm_data)): 93 | 94 | # Current speed 95 | utm_cur = np.array(self.utm_data[i][0:2]) 96 | utm_prev = np.array(self.utm_data[i - 1][0:2]) 97 | 98 | dt = self.utm_data[i][4] - self.utm_data[i - 1][4] 99 | dx = utm_cur[0] - utm_prev[0] 100 | dy = utm_cur[1] - utm_prev[1] 101 | 102 | sp = 0 103 | if (dt > 0): 104 | sp = np.sqrt(dx * dx + dy * dy) / dt 105 | 106 | if (abs(sp - self.avgSpeed) > 2 * self.varSpeed): 107 | self.gnss_data.pop(i) 108 | self.utm_data.pop(i) 109 | i -= 1 110 | 111 | i += 1 112 | 113 | # If data has been removed recompute the avgSpeed and varSpeed 114 | if (ln_bef - len(self.utm_data) > 0): 115 | self.outliers += ln_bef - len(self.utm_data) 116 | self.RemoveOutliers() 117 | 118 | ##### Plot the data ##### 119 | def PlotData(self, data = []): 120 | ''' Plot the data ''' 121 | if (data == []): 122 | lat = [i[0] for i in self.gnss_data] 123 | lon = [i[1] for i in self.gnss_data] 124 | else: 125 | lat = [i[0] for i in data] 126 | lon = [i[1] for i in data] 127 | 128 | plt.plot(lon, lat) 129 | plt.axis('equal') 130 | plt.show() 131 | 132 | ################################################################################################### 133 | if __name__ == '__main__': 134 | if len(sys.argv) < 2: 135 | print('Usage: SimplifyTrack.py rosbag') 136 | else: 137 | ST = SimplifyTrack(sys.argv[1]) -------------------------------------------------------------------------------- /src/simpleDrone.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # import ROS libraries 4 | import rospy 5 | import mavros 6 | from mavros.utils import * 7 | from mavros import setpoint as SP 8 | import mavros.setpoint 9 | import mavros.command 10 | import mavros_msgs.msg 11 | import mavros_msgs.srv 12 | from sensor_msgs.msg import NavSatFix, Imu 13 | from mavros_msgs.srv import CommandLong 14 | from uav_mavros_simulation.srv import target_global_pos, target_global_posResponse, target_local_pos, target_local_posResponse, \ 15 | goto_aruco, goto_arucoResponse, land_aruco, land_arucoResponse 16 | import sys 17 | import signal 18 | from geometry_msgs.msg import Vector3, TwistStamped 19 | from std_msgs.msg import Float32 20 | import math 21 | import numpy as np 22 | import utm 23 | import tf 24 | 25 | 26 | class SimpleDrone(): 27 | """ 28 | Class that connects with a drone using mavros at the gazebo simulation 29 | """ 30 | 31 | def __init__(self): 32 | 33 | # Initialize position variables 34 | self.local_pos = [0.0] * 4 35 | self.global_pos = [0.0] * 4 36 | 37 | rospy.init_node("uav_node") 38 | 39 | self.rate = rospy.Rate(20) 40 | mavros.set_namespace('mavros') 41 | 42 | signal.signal(signal.SIGINT, self.signal_handler) 43 | 44 | self.current_pose = Vector3() 45 | self.UAV_state = mavros_msgs.msg.State() 46 | 47 | # setup subscribers 48 | # /mavros/state 49 | state_sub = rospy.Subscriber(mavros.get_topic('state'), 50 | mavros_msgs.msg.State, self._state_callback) 51 | 52 | # /mavros/local_position/pose 53 | local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), 54 | SP.PoseStamped, self._local_position_callback) 55 | 56 | # /mavros/setpoint_raw/target_local 57 | setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), 58 | mavros_msgs.msg.PositionTarget, self._setpoint_position_callback) 59 | # /mavros/global_position/global 60 | global_position_sub = rospy.Subscriber('mavros/global_position/global', 61 | NavSatFix, self._global_position_callback) 62 | # /mavros/imu/data 63 | rospy.Subscriber('/mavros/imu/data', Imu, self.IMU_callback) 64 | 65 | # setup publisher 66 | # /mavros/setpoint/position/local 67 | self.setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) 68 | # /mavros/setpoint_velocity/cmd_vel 69 | self.cmd_vel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) 70 | 71 | # setup services 72 | # /mavros/cmd/arming 73 | self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) 74 | # mavros/cmd/command 75 | self.command_srv = rospy.ServiceProxy('mavros/cmd/command', mavros_msgs.srv.CommandLong) 76 | # /mavros/set_mode 77 | self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 78 | # goto_pos_services 79 | self.goto_glo_pos_serv = rospy.ServiceProxy("target_global_pos", target_global_pos) 80 | self.goto_loc_pos_serv = rospy.ServiceProxy("target_local_pos", target_local_pos) 81 | # aruco based services 82 | self.goto_aruco_serv = rospy.ServiceProxy('goto_aruco', goto_aruco) 83 | self.land_aruco_serv = rospy.ServiceProxy('land_aruco', land_aruco) 84 | 85 | self.setpoint_msg = mavros.setpoint.PoseStamped( 86 | header=mavros.setpoint.Header( 87 | frame_id="att_pose", 88 | stamp=rospy.Time.now()), 89 | ) 90 | 91 | # wait for FCU connection 92 | while (not self.UAV_state.connected): 93 | self.rate.sleep() 94 | 95 | rospy.loginfo("Uav was successfully connected") 96 | 97 | self.InputHandler() 98 | 99 | #rospy.spin() 100 | 101 | def signal_handler(self, signal, frame): 102 | print('You pressed Ctrl+C!') 103 | sys.exit(0) 104 | 105 | def IMU_callback(self, data): 106 | ''' IMU data subscriber that triggers the parachute in case of failure''' 107 | # Read the linear_accelaration on z-axis 108 | acc_z = data.linear_acceleration.z 109 | 110 | # if the acceleration is too low triger the parachute 111 | if acc_z < 6.0: 112 | self.command_srv(broadcast=False, command=185, param1=1.0) 113 | 114 | def _state_callback(self, topic): 115 | self.UAV_state.armed = topic.armed 116 | self.UAV_state.connected = topic.connected 117 | self.UAV_state.mode = topic.mode 118 | self.UAV_state.guided = topic.guided 119 | 120 | def _setpoint_position_callback(self, topic): 121 | pass 122 | 123 | def _local_position_callback(self, topic): 124 | # Position data 125 | self.local_pos[0] = topic.pose.position.x 126 | self.local_pos[1] = topic.pose.position.y 127 | self.local_pos[2] = topic.pose.position.z 128 | 129 | # Orientation data 130 | (r, p, y) = tf.transformations.euler_from_quaternion([topic.pose.orientation.x, topic.pose.orientation.y, topic.pose.orientation.z, topic.pose.orientation.w]) 131 | self.local_pos[3] = y 132 | self.global_pos[3] = y 133 | 134 | def _global_position_callback(self, data): 135 | self.global_pos[0] = data.latitude 136 | self.global_pos[1] = data.longitude 137 | self.global_pos[2] = data.altitude 138 | 139 | def _set_pose(self, pose, x, y, z): 140 | pose.pose.position.x = x 141 | pose.pose.position.y = y 142 | pose.pose.position.z = z 143 | pose.header = mavros.setpoint.Header( 144 | frame_id="att_pose", 145 | stamp=rospy.Time.now()) 146 | 147 | def update_setpoint(self): 148 | pass 149 | 150 | def SimpleMovement(self): 151 | """ Perform a circlular movement for 20 seconds """ 152 | 153 | self.set_mode(0, 'OFFBOARD') 154 | if (not self.UAV_state.armed): 155 | mavros.command.arming(True) 156 | 157 | last_request = rospy.Time.now() 158 | t0 = last_request 159 | # enter the main loop 160 | while (rospy.Time.now() - t0 < rospy.Duration(20)): 161 | # print "Entered whiled loop" 162 | if (self.UAV_state.mode != "OFFBOARD" and 163 | (rospy.Time.now() - last_request > rospy.Duration(5.0))): 164 | self.set_mode(0, 'OFFBOARD') 165 | print("enabling offboard mode") 166 | last_request = rospy.Time.now() 167 | else: 168 | if (not self.UAV_state.armed and 169 | (rospy.Time.now() - last_request > rospy.Duration(5.0))): 170 | if (mavros.command.arming(True)): 171 | print("Vehicle armed") 172 | last_request = rospy.Time.now() 173 | 174 | #setpoint_msg.pose.position.z = 3 + 2*math.sin(rospy.get_time() * 0.2) 175 | 176 | # Make a circle 177 | self.setpoint_msg.pose.position.x = 2 * math.sin(rospy.get_time() * 0.2) 178 | self.setpoint_msg.pose.position.y = 2 * math.cos(rospy.get_time() * 0.2) 179 | #print("Height: %f" % self.setpoint_msg.pose.position.z) 180 | self.setpoint_local_pub.publish(self.setpoint_msg) 181 | self.rate.sleep() 182 | 183 | def Hover(self): 184 | """ 185 | Used to keep the altitude and position of the drone stable once activated 186 | """ 187 | print("Hover", self.set_mode(0, 'AUTO.LOITER')) 188 | 189 | def GotoPos(self, pos, pos_type = "local"): 190 | """ 191 | Gets the drone to the defined pos, 192 | @pos_type = "local" for movement on the local coordinate system, pos[4] = [x, y, z, Yaw] 193 | @pos_type = "global" for movement using the global coordinate system pos[4] = [Lat, Lon, Alt, Yaw] 194 | """ 195 | 196 | # Deactivate the hover function if activated 197 | dist = -1 198 | if (pos_type == "local"): 199 | loc_pos = mavros_msgs.msg.PositionTarget( 200 | header=mavros.setpoint.Header( 201 | frame_id="att_pose", 202 | stamp=rospy.Time.now()), 203 | ) 204 | # Position 205 | loc_pos.position.x = pos[0] 206 | loc_pos.position.y = pos[1] 207 | loc_pos.position.z = pos[2] 208 | 209 | # Orientation 210 | loc_pos.yaw = pos[3] 211 | 212 | # Call the service 213 | try: 214 | dist = self.goto_loc_pos_serv(loc_pos) 215 | except rospy.ServiceException as exc: 216 | print("Service did not process request: " + str(exc)) 217 | print("Service buisy") 218 | 219 | if (pos_type == "global"): 220 | glob_pos = mavros_msgs.msg.GlobalPositionTarget( 221 | header=mavros.setpoint.Header( 222 | frame_id="att_pose", 223 | stamp=rospy.Time.now()), 224 | ) 225 | 226 | # Position 227 | glob_pos.latitude = pos[0] 228 | glob_pos.longitude = pos[1] 229 | glob_pos.altitude = pos[2] 230 | 231 | # Orientation 232 | glob_pos.yaw = pos[3] 233 | try: 234 | dist = self.goto_glo_pos_serv(glob_pos) 235 | except rospy.ServiceException as exc: 236 | print("Service did not process request: " + str(exc)) 237 | print("Service buisy") 238 | 239 | return dist 240 | 241 | def Land(self, pos = []): 242 | ''' Land the UAV at its current\given position ''' 243 | if len(pos) > 0: 244 | self.GotoPos(pos) 245 | 246 | self.set_mode(0, 'AUTO.LAND') 247 | #mavros.command.arming(False) 248 | 249 | def Return(self): 250 | ''' Return to the home pos and land''' 251 | self.set_mode(0, 'AUTO.RTL') 252 | #mavros.command.arming(False) 253 | 254 | def Help(self): 255 | """ Print the instructions the swarm commander recognizes""" 256 | print("----- Swarm commander available inputs -----") 257 | print("exit -> to close the swarm commander") 258 | print("goto x y z yaw -> uav will go to the specified location location") 259 | print("goto aruco [timeout] -> the uav will got to the aruco marker, timeOut is optional") 260 | print("land [x y z yaw] -> uav will land at its current position or at the optional x, y, z, yaw") 261 | print("land aruco -> uav will land on the aruco marker") 262 | print("return -> uav will return to its home position and land") 263 | print("--------------------------------------------") 264 | 265 | def InputHandler(self): 266 | """ Used to handle the keyboard inputs from the terminal""" 267 | done = False 268 | WirePos = mavros_msgs.msg.PositionTarget() 269 | swarmEffect = False 270 | while not done: 271 | inp = raw_input("Type your command > ") 272 | inp = inp.split() 273 | print(len(inp), (inp)) 274 | 275 | if len(inp) == 1: 276 | if inp[0] == 'exit': 277 | done = True 278 | elif inp[0] == 'return': 279 | self.Return() 280 | elif inp[0] == 'land' and len(inp) == 1: 281 | self.Land() 282 | else: 283 | self.Help() 284 | elif len(inp) > 1: 285 | pos = [] 286 | if (len(inp) == 5): 287 | pos.append(float(inp[1])) 288 | pos.append(float(inp[2])) 289 | pos.append(float(inp[3])) 290 | pos.append(float(inp[4])) 291 | 292 | if inp[0] == 'goto': 293 | if len(inp) == 5: 294 | dist = self.GotoPos(pos) 295 | self.Hover() 296 | elif inp[1] == 'aruco': 297 | timeOut = 60 298 | if len(inp) == 3: 299 | timeOut = inp[2] 300 | dist = self.goto_aruco_serv(timeOut) 301 | self.Hover() 302 | else: 303 | self.Help() 304 | continue 305 | print("Position reached, distance ", dist) 306 | elif inp[0] == 'land': 307 | if len(inp) == 5: 308 | self.Land(pos) 309 | elif inp[1] == 'aruco': 310 | # First go to the aruco marker 311 | dist = self.goto_aruco_serv(60) 312 | rospy.loginfo('Distance to marker ' + str(dist.dist)) 313 | if dist.dist < 1.0: 314 | self.land_aruco_serv(60) 315 | self.Land() 316 | else: 317 | rospy.logerr('Marker too far') 318 | self.Hover() 319 | continue 320 | else: 321 | self.Help() 322 | else: 323 | self.Help() 324 | else: 325 | self.Help() 326 | 327 | ################################################################################################### 328 | if __name__ == "__main__": 329 | SD = SimpleDrone() 330 | #SD2 = SimpleDrone(2) 331 | #SD2.GotoLocalPos(5, 5, 10) 332 | #SD2.SimpleMovement() 333 | #SD1.GotoGlobPos(55.43600, 10.46091, 540) -------------------------------------------------------------------------------- /srv/goto_aruco.srv: -------------------------------------------------------------------------------- 1 | float64 timeOut 2 | --- 3 | float64 dist -------------------------------------------------------------------------------- /srv/land_aruco.srv: -------------------------------------------------------------------------------- 1 | float64 timeOut 2 | --- 3 | float64 dist -------------------------------------------------------------------------------- /srv/target_global_pos.srv: -------------------------------------------------------------------------------- 1 | mavros_msgs/GlobalPositionTarget goal_pos 2 | --- 3 | float64 dist -------------------------------------------------------------------------------- /srv/target_local_pos.srv: -------------------------------------------------------------------------------- 1 | mavros_msgs/PositionTarget goal_pos 2 | --- 3 | float64 dist -------------------------------------------------------------------------------- /worlds/aruco.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 15 | model://aruco_marker 16 | 4 4 0.1 0 0 0 17 | 18 | 19 | 20 | 21 | --------------------------------------------------------------------------------