├── CMakeLists.txt ├── README.md ├── launch ├── mavros_mission_apm.launch └── mavros_mission_px4.launch ├── missions ├── missionwp_file.txt ├── missionwp_file_AlaFija.txt ├── missionwp_file_Drone_1.txt ├── missionwp_file_Drone_2.txt ├── missionwp_file_Drone_3.txt └── missionwp_qgroundcontrol.txt ├── package.xml └── src ├── mavros_mission_apm.py └── mavros_mission_px4.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mavros_auto_mission) 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 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES mavros_mission_px4 107 | # CATKIN_DEPENDS rospy std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/mavros_mission_px4.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/mavros_mission_px4_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables and/or libraries for installation 167 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 168 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark cpp header files for installation 174 | # install(DIRECTORY include/${PROJECT_NAME}/ 175 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 176 | # FILES_MATCHING PATTERN "*.h" 177 | # PATTERN ".svn" EXCLUDE 178 | # ) 179 | 180 | ## Mark other files for installation (e.g. launch and bag files, etc.) 181 | # install(FILES 182 | # # myfile1 183 | # # myfile2 184 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 185 | # ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mavros_mission_px4.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mavros_auto_mission # 2 | 3 | ROS package (tested in ROS Kinetic) for sending a mission via MavLINK to an aerial vehicle with a PX4 or an ArduPilot autopilot system. It is also able to auto remove failsafes, arm the motors, enter Mission mode and complete the mission autonomously. 4 | 5 | Test with a simulated PX4 fixed-wing aircraft in this video: 6 | 7 | [![Video](https://img.youtube.com/vi/jDYtM7kgN5o/maxresdefault.jpg)](https://youtu.be/jDYtM7kgN5o) 8 | 9 | 10 | ## Dependencies ## 11 | 12 | Besides ROS, both [MavROS](http://wiki.ros.org/mavros) and [PX4 Firmware](https://dev.px4.io/v1.9.0/en/setup/dev_env.html) must be installed 13 | 14 | 15 | ## Usage ## 16 | ### With PX4 SITL and Gazebo ### 17 | 18 | 1. Update your takeoff location via environment variables as explained [here](http://dev.px4.io/v1.9.0/en/simulation/gazebo.html#set-custom-takeoff-location). For the RC airfield in the video, the coordinates are: 19 | 20 | ```sh 21 | export PX4_HOME_LAT=40.091754 22 | export PX4_HOME_LON=-3.695714 23 | ``` 24 | 25 | 2. Launch PX4 and MavROS: 26 | ```sh 27 | roslaunch px4 mavros_posix_sitl.launch 28 | 29 | ``` 30 | 31 | 3. Launch mavros_auto_mission: 32 | ```sh 33 | roslaunch mavros_auto_mission mavros_mission_px4.launch 34 | 35 | ``` 36 | ### With ArduPilot in a real aircraft ### 37 | 38 | 1. Connect to the real ArduPilot (Copter, Plane or Rover) via MavLink using a telemetry radio (3DR or similar). 39 | 40 | 2. Launch mavros_auto_mission: 41 | ```sh 42 | roslaunch mavros_auto_mission mavros_mission_apm.launch 43 | 44 | 45 | -------------------------------------------------------------------------------- /launch/mavros_mission_apm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/mavros_mission_px4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /missions/missionwp_file.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 120 2 | 1 1 3 22 15.0 0.0 0.0 nan 40.092141 -3.692452 30.0 1 3 | 2 0 3 16 0.0 0.0 0.0 nan 40.093716 -3.692720 30.0 1 4 | 3 0 3 16 10.0 0.0 0.0 nan 40.093051 -3.699953 20.0 1 5 | 4 0 3 16 5.0 0.0 0.0 nan 40.091560 -3.699741 10.0 1 6 | 5 0 3 21 0.0 0.0 0.0 nan 40.091738 -3.695878 0 1 7 | -------------------------------------------------------------------------------- /missions/missionwp_file_AlaFija.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 120 2 | 1 0 3 22 15.0 0.0 0.0 nan 40.092141 -3.692452 30.0 1 3 | 2 0 3 16 0.0 0.0 0.0 nan 40.093716 -3.692720 30.0 1 4 | 3 0 3 16 10.0 0.0 0.0 nan 40.093051 -3.699953 20.0 1 5 | 4 0 3 16 5.0 0.0 0.0 nan 40.091560 -3.699741 10.0 1 6 | 5 0 3 21 0.0 0.0 0.0 nan 40.091738 -3.695878 0 1 7 | -------------------------------------------------------------------------------- /missions/missionwp_file_Drone_1.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 120 2 | 0 1 2 178 1.0 4.0 -1.0 0.0 0.0 0.0 0.0 1 3 | 1 0 3 22 15.0 0.0 0.0 nan 40.09175109863281 -3.6957149505615234 5.0 1 4 | 2 0 3 16 0.0 0.0 0.0 nan 40.091854095458984 -3.6949193477630615 10.0 1 5 | 3 0 3 16 0.0 0.0 0.0 nan 40.09211349487305 -3.694973945617676 15.0 1 6 | 4 0 3 16 10.0 0.0 0.0 nan 40.09221267700195 -3.695798397064209 15.0 1 7 | 5 0 3 16 0.0 0.0 0.0 nan 40.0919303894043 -3.6966123580932617 15.0 1 8 | 6 0 3 16 0.0 0.0 0.0 nan 40.091644287109375 -3.6965627670288086 10.0 1 9 | 7 0 3 21 0.0 0.0 0.0 nan 40.09172439575195 -3.6959054470062256 5.0 1 10 | -------------------------------------------------------------------------------- /missions/missionwp_file_Drone_2.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 120 2 | 0 1 2 178 1.0 8.0 -1.0 0.0 0.0 0.0 0.0 1 3 | 1 0 3 22 15.0 0.0 0.0 nan 40.09175109863281 -3.6957149505615234 5.0 1 4 | 2 0 3 16 0.0 0.0 0.0 nan 40.091854095458984 -3.6949193477630615 10.0 1 5 | 3 0 3 16 0.0 0.0 0.0 nan 40.09211349487305 -3.694973945617676 15.0 1 6 | 4 0 3 16 10.0 0.0 0.0 nan 40.09221267700195 -3.695798397064209 15.0 1 7 | 5 0 3 16 0.0 0.0 0.0 nan 40.0919303894043 -3.6966123580932617 15.0 1 8 | 6 0 3 16 0.0 0.0 0.0 nan 40.091644287109375 -3.6965627670288086 10.0 1 9 | 7 0 3 21 0.0 0.0 0.0 nan 40.09172439575195 -3.6959054470062256 5.0 1 10 | -------------------------------------------------------------------------------- /missions/missionwp_file_Drone_3.txt: -------------------------------------------------------------------------------- 1 | QGC WPL 120 2 | 0 1 2 178 1.0 2.0 -1.0 0.0 0.0 0.0 0.0 1 3 | 1 0 3 22 15.0 0.0 0.0 nan 40.09175109863281 -3.6957149505615234 5.0 1 4 | 2 0 3 16 0.0 0.0 0.0 nan 40.091854095458984 -3.6949193477630615 7.0 1 5 | 3 0 2 178 1.0 3.0 -1.0 0.0 0.0 0.0 0.0 1 6 | 4 0 3 16 0.0 0.0 0.0 nan 40.09211349487305 -3.694973945617676 10.0 1 7 | 5 0 3 16 0.0 0.0 0.0 nan 40.0919303894043 -3.6966123580932617 8.0 1 8 | 6 0 3 16 0.0 0.0 0.0 nan 40.091644287109375 -3.6965627670288086 6.0 1 9 | 7 0 3 21 0.0 0.0 0.0 nan 40.09172439575195 -3.6959054470062256 5.0 1 10 | -------------------------------------------------------------------------------- /missions/missionwp_qgroundcontrol.txt: -------------------------------------------------------------------------------- 1 | { 2 | "fileType": "Plan", 3 | "geoFence": { 4 | "circles": [ 5 | ], 6 | "polygons": [ 7 | ], 8 | "version": 2 9 | }, 10 | "groundStation": "QGroundControl", 11 | "mission": { 12 | "cruiseSpeed": 15, 13 | "firmwareType": 12, 14 | "hoverSpeed": 5, 15 | "items": [ 16 | { 17 | "autoContinue": true, 18 | "command": 530, 19 | "doJumpId": 1, 20 | "frame": 2, 21 | "params": [ 22 | 0, 23 | 0, 24 | null, 25 | null, 26 | null, 27 | null, 28 | null 29 | ], 30 | "type": "SimpleItem" 31 | }, 32 | { 33 | "autoContinue": true, 34 | "command": 2000, 35 | "doJumpId": 2, 36 | "frame": 2, 37 | "params": [ 38 | 0, 39 | 0, 40 | 1, 41 | null, 42 | null, 43 | null, 44 | null 45 | ], 46 | "type": "SimpleItem" 47 | }, 48 | { 49 | "autoContinue": true, 50 | "command": 178, 51 | "doJumpId": 3, 52 | "frame": 2, 53 | "params": [ 54 | 1, 55 | 1, 56 | -1, 57 | 0, 58 | 0, 59 | 0, 60 | 0 61 | ], 62 | "type": "SimpleItem" 63 | }, 64 | { 65 | "AMSLAltAboveTerrain": null, 66 | "Altitude": 5, 67 | "AltitudeMode": 1, 68 | "autoContinue": true, 69 | "command": 22, 70 | "doJumpId": 4, 71 | "frame": 3, 72 | "params": [ 73 | 15, 74 | 0, 75 | 0, 76 | null, 77 | 40.0918506320164, 78 | -3.6950724186136767, 79 | 5 80 | ], 81 | "type": "SimpleItem" 82 | }, 83 | { 84 | "AMSLAltAboveTerrain": null, 85 | "Altitude": 5, 86 | "AltitudeMode": 1, 87 | "autoContinue": true, 88 | "command": 16, 89 | "doJumpId": 5, 90 | "frame": 3, 91 | "params": [ 92 | 0, 93 | 0, 94 | 0, 95 | null, 96 | 40.09199372, 97 | -3.6957433, 98 | 5 99 | ], 100 | "type": "SimpleItem" 101 | }, 102 | { 103 | "autoContinue": true, 104 | "command": 178, 105 | "doJumpId": 6, 106 | "frame": 2, 107 | "params": [ 108 | 1, 109 | 2, 110 | -1, 111 | 0, 112 | 0, 113 | 0, 114 | 0 115 | ], 116 | "type": "SimpleItem" 117 | }, 118 | { 119 | "AMSLAltAboveTerrain": null, 120 | "Altitude": 5, 121 | "AltitudeMode": 1, 122 | "autoContinue": true, 123 | "command": 16, 124 | "doJumpId": 7, 125 | "frame": 3, 126 | "params": [ 127 | 0, 128 | 0, 129 | 0, 130 | null, 131 | 40.09168389, 132 | -3.69639719, 133 | 5 134 | ], 135 | "type": "SimpleItem" 136 | }, 137 | { 138 | "autoContinue": true, 139 | "command": 178, 140 | "doJumpId": 8, 141 | "frame": 2, 142 | "params": [ 143 | 1, 144 | 3, 145 | -1, 146 | 0, 147 | 0, 148 | 0, 149 | 0 150 | ], 151 | "type": "SimpleItem" 152 | }, 153 | { 154 | "AMSLAltAboveTerrain": null, 155 | "Altitude": 0, 156 | "AltitudeMode": 1, 157 | "autoContinue": true, 158 | "command": 21, 159 | "doJumpId": 9, 160 | "frame": 3, 161 | "params": [ 162 | 0, 163 | 0, 164 | 0, 165 | null, 166 | 40.09170635734441, 167 | -3.6960942083062207, 168 | 0 169 | ], 170 | "type": "SimpleItem" 171 | } 172 | ], 173 | "plannedHomePosition": [ 174 | 40.0917044, 175 | -3.6960919, 176 | 487.97 177 | ], 178 | "vehicleType": 2, 179 | "version": 2 180 | }, 181 | "rallyPoints": { 182 | "points": [ 183 | ], 184 | "version": 2 185 | }, 186 | "version": 1 187 | } 188 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mavros_auto_mission 4 | 0.0.1 5 | The mavros_auto_mission package 6 | 7 | 8 | Diego Martin 9 | 10 | 11 | 12 | 13 | 14 | TODO 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | catkin 50 | rospy 51 | std_msgs 52 | rospy 53 | std_msgs 54 | rospy 55 | std_msgs 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /src/mavros_mission_apm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ########################################################################## 4 | # JdeRobot. APM + MAVROS: Load a mission using WPs file + fly fully AUTO 5 | # 6 | # Diego Martin 7 | # 24/02/2019 8 | # v1.0 9 | ########################################################################### 10 | 11 | import rospy 12 | import time 13 | import os 14 | import mavros 15 | from mavros import command 16 | from mavros.utils import * 17 | from mavros_msgs.msg import * 18 | from mavros_msgs.srv import * 19 | 20 | mavros.set_namespace() 21 | 22 | # Class to manage flight mode (APM stack). 23 | class apmFlightMode: 24 | 25 | def __init__(self): 26 | pass 27 | 28 | def setTakeoff(self): 29 | rospy.wait_for_service('mavros/cmd/takeoff') 30 | try: 31 | takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL) 32 | takeoffService(altitude = 2.5) 33 | rospy.loginfo("Taking off") 34 | except rospy.ServiceException, e: 35 | rospy.logerror("Takeoff failed: %s"%e) 36 | 37 | 38 | def setAutoLandMode(self): 39 | rospy.wait_for_service('mavros/set_mode') 40 | try: 41 | flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 42 | flightModeService(custom_mode='LAND') 43 | rospy.loginfo("Landing") 44 | except rospy.ServiceException, e: 45 | rospy.logerror("Landing failed: %s. Land Mode could not be set"%e) 46 | 47 | def setArm(self): 48 | rospy.wait_for_service('mavros/cmd/arming') 49 | try: 50 | armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) 51 | armService(True) 52 | rospy.loginfo("Arming motors command sent OK") # It doens't mean the copter arms, just the command is sent properly. Add checking arming 53 | except rospy.ServiceException, e: 54 | rospy.logerror("Arming motors command sending failed: %s"%e) 55 | 56 | def loadMission(self): 57 | # Load mission from file (MP format). 58 | # To do: change to avoid using os.system + mavwp 59 | # Handle errors pending! 60 | os.system("rosrun mavros mavwp load ~/catkin_ws/src/mavros_auto_mission/missions/missionwp_file.txt") # Load new mission 61 | rospy.loginfo("Mission WayPoints LOADED!") 62 | os.system("rosrun mavros mavwp show") # Show mission WP loaded 63 | 64 | 65 | def setAutoMissionMode(self): 66 | rospy.wait_for_service('mavros/set_mode') 67 | try: 68 | flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 69 | flightModeService(custom_mode='AUTO') 70 | rospy.loginfo("Entering AUTO mode OK") 71 | except rospy.ServiceException, e: 72 | rospy.logerror("Entering AUTO failed: %s. AUTO mode could not be set."%e) 73 | 74 | 75 | def read_failsafe(self): 76 | try: 77 | # Gets the current values of PX4 failsafe-related parameters 78 | get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet) 79 | DLL_param = get(param_id="NAV_DLL_ACT") # Datalink Failsafe PX4 Parameter 80 | RCL_param = get(param_id="NAV_RCL_ACT") # RC Failsafe PX4 Parameter 81 | print " " 82 | print "----------PX4 FAILSAFE STATUS--------------" 83 | print " Present NAV_DLL_ACT value is", DLL_param.value.integer 84 | print " Present NAV_RCL_ACT value is", RCL_param.value.integer 85 | print "-------------------------------------------" 86 | print " " 87 | return {'DL':DLL_param.value.integer,'RC':RCL_param.value.integer} 88 | except rospy.ServiceException, e: 89 | rospy.logerror("Failsafe Status read failed: %s"%e) 90 | 91 | def remove_failsafe(self): 92 | try: 93 | # Disables both Datalink and RC failsafes 94 | val = ParamValue(integer=0, real=0.0) # Int value for disabling failsafe 95 | set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet) 96 | new_DLL_param = set(param_id="NAV_DLL_ACT", value=val) 97 | new_RCL_param = set(param_id="NAV_RCL_ACT", value=val) 98 | print " " 99 | print "----------REMOVING PX4 FAILSAFES ----------" 100 | print " New NAV_DLL_ACT value is", new_DLL_param.value.integer 101 | print " New NAV_RCL_ACT value is", new_RCL_param.value.integer 102 | print "--------------------------------------------" 103 | print " " 104 | except rospy.ServiceException, e: 105 | rospy.logerror("Failsafe Status change failed: %s"%e) 106 | 107 | 108 | # WP reached Callback function. Controls spam by publishing once per WP! 109 | def WP_callback(msg): 110 | 111 | global last_wp # Maybe there is a more elegant way of doint this :-) 112 | global starting_time 113 | global mission_started # Bool handles reaching WP#0 twice (mission beginning and end) 114 | 115 | try: 116 | mission_started 117 | except NameError: # Mission begins 118 | rospy.loginfo("Starting MISSION: Waypoint #0 reached") 119 | starting_time = msg.header.stamp.secs 120 | mission_started = True 121 | else: # Mission ongoing 122 | if msg.wp_seq == 0 and msg.wp_seq != last_wp: # Returning to WP #0 (last) 123 | elapsed_time = msg.header.stamp.secs-starting_time 124 | rospy.loginfo("Ending MISSION: Total time: %d s", elapsed_time) 125 | elif msg.wp_seq != last_wp: # Intermediate WPs 126 | elapsed_time = msg.header.stamp.secs-starting_time 127 | rospy.loginfo("MISSION Waypoint #%s reached. Elapsed time: %d s", msg.wp_seq, elapsed_time) 128 | 129 | # Update last WP reached 130 | last_wp=msg.wp_seq 131 | 132 | 133 | # Main Function 134 | def main(): 135 | 136 | rospy.init_node('auto_mission_node', anonymous=True) 137 | 138 | # Flight mode object 139 | APMmodes = apmFlightMode() 140 | 141 | # Read Datalink and RC failsafe STATUS. Remove if present (for SITL)! 142 | failsafe_status = APMmodes.read_failsafe() 143 | #if (failsafe_status['DL'] != 0) or (failsafe_status['RC'] != 0): 144 | # APMmodes.remove_failsafe() 145 | 146 | # AUTO MISSION: set mode, read WPs and Arm! 147 | APMmodes.loadMission() 148 | APMmodes.setArm() # Previous to AUTO! 149 | APMmodes.setAutoMissionMode() 150 | 151 | # Subscribe to drone state to publish mission updates 152 | sub=rospy.Subscriber('mavros/mission/reached', WaypointReached, WP_callback) 153 | 154 | # Keep main loop 155 | rospy.spin() 156 | 157 | 158 | if __name__ == '__main__': 159 | try: 160 | main() 161 | except rospy.ROSInterruptException: 162 | pass 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | -------------------------------------------------------------------------------- /src/mavros_mission_px4.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ########################################################################## 4 | # JdeRobot. PX4 + MAVROS: Load a mission using WPs file + fly fully AUTO 5 | # 6 | # Diego Martin 7 | # 09/02/2019 8 | # v1.0 9 | ########################################################################### 10 | 11 | import rospy 12 | import time 13 | import os 14 | import mavros 15 | from mavros import command 16 | from mavros.utils import * 17 | from mavros_msgs.msg import * 18 | from mavros_msgs.srv import * 19 | 20 | mavros.set_namespace() 21 | 22 | # Class to manage flight mode (PX4 stack). 23 | class px4FlightMode: 24 | 25 | def __init__(self): 26 | pass 27 | 28 | def setTakeoff(self): 29 | rospy.wait_for_service('mavros/cmd/takeoff') 30 | try: 31 | takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL) 32 | takeoffService(altitude = 2.5) 33 | rospy.loginfo("Taking off") 34 | except rospy.ServiceException, e: 35 | rospy.logerror("Takeoff failed: %s"%e) 36 | 37 | 38 | def setAutoLandMode(self): 39 | rospy.wait_for_service('mavros/set_mode') 40 | try: 41 | flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 42 | flightModeService(custom_mode='AUTO.LAND') 43 | rospy.loginfo("Landing") 44 | except rospy.ServiceException, e: 45 | rospy.logerror("Landing failed: %s. Autoland Mode could not be set"%e) 46 | 47 | def setArm(self): 48 | rospy.wait_for_service('mavros/cmd/arming') 49 | try: 50 | armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) 51 | armService(True) 52 | rospy.loginfo("Arming motors OK") 53 | except rospy.ServiceException, e: 54 | rospy.logerror("Arming motors failed: %s"%e) 55 | 56 | def loadMission(self): 57 | # Load mission from file (MP format). 58 | # To do: change to avoid using os.system + mavwp 59 | # Handle errors pending! 60 | os.system("rosrun mavros mavwp load ~/catkin_ws/src/mavros_auto_mission/missions/missionwp_file.txt") # Load new mission 61 | rospy.loginfo("Mission WayPoints LOADED!") 62 | os.system("rosrun mavros mavwp show") # Show mission WP loaded 63 | 64 | 65 | def setAutoMissionMode(self): 66 | rospy.wait_for_service('mavros/set_mode') 67 | try: 68 | flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) 69 | flightModeService(custom_mode='AUTO.MISSION') 70 | rospy.loginfo("Entering Auto Mission mode OK") 71 | except rospy.ServiceException, e: 72 | rospy.logerror("Entering Auto Mission failed: %s. AUTO.MISSION mode could not be set."%e) 73 | 74 | 75 | def read_failsafe(self): 76 | try: 77 | # Gets the current values of PX4 failsafe-related parameters 78 | get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet) 79 | DLL_param = get(param_id="NAV_DLL_ACT") # Datalink Failsafe PX4 Parameter 80 | RCL_param = get(param_id="NAV_RCL_ACT") # RC Failsafe PX4 Parameter 81 | print " " 82 | print "----------PX4 FAILSAFE STATUS--------------" 83 | print " Present NAV_DLL_ACT value is", DLL_param.value.integer 84 | print " Present NAV_RCL_ACT value is", RCL_param.value.integer 85 | print "-------------------------------------------" 86 | print " " 87 | return {'DL':DLL_param.value.integer,'RC':RCL_param.value.integer} 88 | except rospy.ServiceException, e: 89 | rospy.logerror("Failsafe Status read failed: %s"%e) 90 | 91 | def remove_failsafe(self): 92 | try: 93 | # Disables both Datalink and RC failsafes 94 | val = ParamValue(integer=0, real=0.0) # Int value for disabling failsafe 95 | set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet) 96 | new_DLL_param = set(param_id="NAV_DLL_ACT", value=val) 97 | new_RCL_param = set(param_id="NAV_RCL_ACT", value=val) 98 | print " " 99 | print "----------REMOVING PX4 FAILSAFES ----------" 100 | print " New NAV_DLL_ACT value is", new_DLL_param.value.integer 101 | print " New NAV_RCL_ACT value is", new_RCL_param.value.integer 102 | print "--------------------------------------------" 103 | print " " 104 | except rospy.ServiceException, e: 105 | rospy.logerror("Failsafe Status change failed: %s"%e) 106 | 107 | 108 | # WP reached Callback function. Controls spam by publishing once per WP! 109 | def WP_callback(msg): 110 | 111 | global last_wp # Maybe there is a more elegant way of doint this :-) 112 | global starting_time 113 | global mission_started # Bool handles reaching WP#0 twice (mission beginning and end) 114 | 115 | try: 116 | mission_started 117 | except NameError: # Mission begins 118 | rospy.loginfo("Starting MISSION: Waypoint #0 reached") 119 | starting_time = msg.header.stamp.secs 120 | mission_started = True 121 | else: # Mission ongoing 122 | if msg.wp_seq == 0 and msg.wp_seq != last_wp: # Returning to WP #0 (last) 123 | elapsed_time = msg.header.stamp.secs-starting_time 124 | rospy.loginfo("Ending MISSION: Total time: %d s", elapsed_time) 125 | elif msg.wp_seq != last_wp: # Intermediate WPs 126 | elapsed_time = msg.header.stamp.secs-starting_time 127 | rospy.loginfo("MISSION Waypoint #%s reached. Elapsed time: %d s", msg.wp_seq, elapsed_time) 128 | 129 | # Update last WP reached 130 | last_wp=msg.wp_seq 131 | 132 | 133 | # Main Function 134 | def main(): 135 | 136 | rospy.init_node('auto_mission_node', anonymous=True) 137 | 138 | # Flight mode object 139 | PX4modes = px4FlightMode() 140 | 141 | # Read Datalink and RC failsafe STATUS. Remove if present (for SITL)! 142 | failsafe_status = PX4modes.read_failsafe() 143 | if (failsafe_status['DL'] != 0) or (failsafe_status['RC'] != 0): 144 | PX4modes.remove_failsafe() 145 | 146 | # AUTO MISSION: set mode, read WPs and Arm! 147 | PX4modes.loadMission() 148 | PX4modes.setAutoMissionMode() 149 | PX4modes.setArm() 150 | 151 | # Subscribe to drone state to publish mission updates 152 | sub=rospy.Subscriber('mavros/mission/reached', WaypointReached, WP_callback) 153 | 154 | # Keep main loop 155 | rospy.spin() 156 | 157 | 158 | if __name__ == '__main__': 159 | try: 160 | main() 161 | except rospy.ROSInterruptException: 162 | pass 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | --------------------------------------------------------------------------------