├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg └── contolParams.yaml ├── init.sh ├── launch ├── apm.launch ├── apmsitl.launch ├── follow.launch ├── followScan.launch ├── offset.launch └── px4flow.launch ├── package.xml └── src ├── darknetSub.cpp ├── examples ├── darknetSub.cpp └── frame.cpp ├── follow.cpp ├── followWithScan.cpp ├── frame.cpp ├── more_logging.cpp ├── pidWaypoint.cpp ├── setDestination.cpp ├── setHeading.cpp ├── setHome.cpp ├── staple.cpp └── velTest.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flight_pkg) 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 | mavros 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | generate_messages( 70 | DEPENDENCIES 71 | std_msgs # Or other packages containing msgs 72 | ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES flight_pkg 106 | CATKIN_DEPENDS mavros 107 | # DEPENDS system_lib 108 | ) 109 | 110 | set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/flight_pkg.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | add_executable(days_before_logs src/more_logging.cpp) 137 | target_link_libraries(days_before_logs ${catkin_LIBRARIES}) 138 | 139 | add_executable(velTest src/velTest.cpp) 140 | target_link_libraries(velTest ${catkin_LIBRARIES}) 141 | 142 | add_executable(pidWaypoint src/pidWaypoint.cpp) 143 | target_link_libraries(pidWaypoint ${catkin_LIBRARIES}) 144 | 145 | add_executable(staple src/staple.cpp) 146 | target_link_libraries(staple ${catkin_LIBRARIES}) 147 | 148 | add_executable(RoombaFollow src/follow.cpp) 149 | target_link_libraries(RoombaFollow ${catkin_LIBRARIES}) 150 | 151 | add_executable(RoombaFollowScan src/followWithScan.cpp) 152 | target_link_libraries(RoombaFollowScan ${catkin_LIBRARIES}) 153 | 154 | #add_executable(darknetSub src/examples/darknetSub.cpp) 155 | #target_link_libraries(darknetSub ${catkin_LIBRARIES}) 156 | 157 | add_executable(setHome src/setHome.cpp) 158 | target_link_libraries(setHome ${catkin_LIBRARIES}) 159 | 160 | add_executable(setDestination src/setDestination.cpp) 161 | target_link_libraries(setDestination ${catkin_LIBRARIES}) 162 | 163 | add_executable(setHeading src/setHeading.cpp) 164 | target_link_libraries(setHeading ${catkin_LIBRARIES}) 165 | 166 | ## Rename C++ executable without prefix 167 | ## The above recommended prefix causes long target names, the following renames the 168 | ## target back to the shorter version for ease of user use 169 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 170 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 171 | 172 | ## Add cmake target dependencies of the executable 173 | ## same as for the library above 174 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 175 | 176 | ## Specify libraries to link a library or executable target against 177 | # target_link_libraries(${PROJECT_NAME}_node 178 | # ${catkin_LIBRARIES} 179 | # ) 180 | 181 | ############# 182 | ## Install ## 183 | ############# 184 | 185 | # all install targets should use catkin DESTINATION variables 186 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 187 | 188 | ## Mark executable scripts (Python etc.) for installation 189 | ## in contrast to setup.py, you can choose the destination 190 | # install(PROGRAMS 191 | # scripts/my_python_script 192 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 193 | # ) 194 | 195 | ## Mark executables and/or libraries for installation 196 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 197 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 198 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 199 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 200 | # ) 201 | 202 | ## Mark cpp header files for installation 203 | # ins://www.google.com/search?client=ubuntu&channel=fs&q=catkin+ignore+clock+skew&ie=utf-8&oe=utf-8#channel=fs&q=catkin+build+not+updating+scriptstall(DIRECTORY include/${PROJECT_NAME}/ 204 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 205 | # FILES_MATCHING PATTERN "*.h" 206 | # PATTERN ".svn" EXCLUDE 207 | # ) 208 | 209 | ## Mark other files for installation (e.g. launch and bag files, etc.) 210 | # install(FILES 211 | # # myfile1 212 | # # myfile2 213 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 214 | # ) 215 | 216 | ############# 217 | ## Testing ## 218 | ############# 219 | 220 | ## Add gtest based cpp test target and link libraries 221 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_flight_pkg.cpp) 222 | # if(TARGET ${PROJECT_NAME}-test) 223 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 224 | # endif() 225 | 226 | ## Add folders to be run by python nosetests 227 | # catkin_add_nosetests(test) 228 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Texas Aerial Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Texas Aerial Robotics ROS package 2 | 3 | In this repository are the files nessary to build our ROS (Robot Operating System) package. 4 | 5 | We use **Ubuntu 18.04** and **ROS Melodic** 6 | 7 | This package is loaded and runs on our Nvidia Jetson TX2 onboard the quadcopter. 8 | 9 | It then feeds commands to the Pixhawk 2 using MAVROS. 10 | 11 | Note that there is a **Troubleshooting** section at the bottom of this page. 12 | 13 | Code blocks are meant to be typed in Terminal windows. "Control+Alt+T" opens a new Terminal window. 14 | 15 | ## 1. Install ROS 16 | 17 | - Do _Desktop Install_ 18 | - Follow until _Step 1.7_ at the end of the page 19 | 20 | First, install **ROS Melodic** using the following instructions: http://wiki.ros.org/melodic/Installation/Ubuntu 21 | 22 | 23 | ## 2. Set Up Catkin workspace 24 | 25 | We use `catkin build` instead of `catkin_make`. Please install the following: 26 | ``` 27 | sudo apt-get install python-wstool python-rosinstall-generator python-catkin-tools 28 | ``` 29 | 30 | Then, initialize the catkin workspace: 31 | ``` 32 | mkdir -p ~/catkin_ws/src 33 | cd ~/catkin_ws 34 | catkin init 35 | ``` 36 | 37 | ## 3. Dependencies installation 38 | 39 | Install `mavros` and `mavlink` from source: 40 | ``` 41 | cd ~/catkin_ws 42 | wstool init ~/catkin_ws/src 43 | 44 | rosinstall_generator --upstream mavros | tee /tmp/mavros.rosinstall 45 | rosinstall_generator mavlink | tee -a /tmp/mavros.rosinstall 46 | wstool merge -t src /tmp/mavros.rosinstall 47 | wstool update -t src 48 | rosdep install --from-paths src --ignore-src --rosdistro `echo $ROS_DISTRO` -y 49 | 50 | catkin build 51 | ``` 52 | 53 | Open your `~/.bashrc` for editing: 54 | ``` 55 | gedit ~/.bashrc 56 | ``` 57 | 58 | Add this line to end of `~/.bashrc`: 59 | ``` 60 | source ~/catkin_ws/devel/setup.bash 61 | ``` 62 | Save and exit. Then run: 63 | ``` 64 | source ~/.bashrc 65 | ``` 66 | 67 | ## 4. Clone our ROS package repository 68 | 69 | ``` 70 | cd ~/catkin_ws/src 71 | git clone https://github.com/Texas-Aerial-Robotics/Controls-ROS.git 72 | ``` 73 | Our repository should now be copied to `~/catkin_ws/src/Controls-ROS/` (don't run this line. This is just saying that if you browse in the file manager, you will see those folders). 74 | 75 | ## 5. Build instructions 76 | Inside `catkin_ws`, run `catkin build`: 77 | 78 | ``` 79 | cd ~/catkin_ws 80 | catkin build 81 | ``` 82 | 83 | ## 6. Install Ardupilot 84 | 85 | In home directory: 86 | ``` 87 | cd ~ 88 | git clone https://github.com/Texas-Aerial-Robotics/ardupilot.git 89 | cd ardupilot 90 | git checkout Copter-3.5 91 | git submodule update --init --recursive 92 | ``` 93 | 94 | Install some packages: 95 | ``` 96 | sudo apt install python-matplotlib python-serial python-wxgtk3.0 python-wxtools python-lxml python-scipy python-opencv ccache gawk git python-pip python-pexpect 97 | sudo pip2 install future pymavlink MAVProxy 98 | ``` 99 | 100 | Open `~/.bashrc` for editing: 101 | ``` 102 | gedit ~/.bashrc 103 | ``` 104 | 105 | Add these lines to end of `~/.bashrc` (the file open in the text editor): 106 | ``` 107 | export PATH=$PATH:$HOME/ardupilot/Tools/autotest 108 | export PATH=/usr/lib/ccache:$PATH 109 | ``` 110 | 111 | Save and close the text editor. 112 | 113 | Reload `~/.bashrc`: 114 | ``` 115 | . ~/.bashrc 116 | ``` 117 | 118 | Run SITL (Software In The Loop) once to set params: 119 | ``` 120 | cd ~/ardupilot/ArduCopter 121 | sim_vehicle.py -w 122 | ``` 123 | 124 | Once the new Terminal window pops up and you see the ArduPilot software say "READY TO FLY," you can kill the process by hitting "Control+C" in the Terminal it is running out of. 125 | 126 | ## 7. Install Gazebo 127 | 128 | Setup your computer to accept software from http://packages.osrfoundation.org: 129 | ``` 130 | sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 131 | ``` 132 | 133 | Setup keys: 134 | ``` 135 | wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - 136 | ``` 137 | 138 | Reload software list: 139 | ``` 140 | sudo apt update 141 | ``` 142 | 143 | Install Gazebo: 144 | ``` 145 | sudo apt install gazebo9 libgazebo9-dev 146 | ``` 147 | 148 | Install ROS plugins: 149 | ``` 150 | sudo apt install ros-melodic-gazebo-ros ros-melodic-gazebo-plugins 151 | ``` 152 | 153 | Get Gazebo plugin for APM (ArduPilot Master): 154 | ``` 155 | cd ~ 156 | git clone https://github.com/SwiftGust/ardupilot_gazebo 157 | cd ardupilot_gazebo 158 | git checkout gazebo9 159 | mkdir build 160 | cd build 161 | cmake .. 162 | make -j4 163 | sudo make install 164 | ``` 165 | 166 | Set paths for models: 167 | ``` 168 | echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/gazebo_models' >> ~/.bashrc 169 | . ~/.bashrc 170 | ``` 171 | 172 | Add Gazebo Models: 173 | ``` 174 | hg clone https://bitbucket.org/osrf/gazebo_models ~/gazebo_ws/gazebo_models 175 | cd ~/gazebo_ws/gazebo_models 176 | echo 'export GAZEBO_MODEL_PATH=~/gazebo_ws/gazebo_models' >> ~/.bashrc 177 | source ~/.bashrc 178 | ``` 179 | 180 | ## 8. Run Simulator 181 | 182 | In one Terminal (Terminal 1), run SITL: 183 | ``` 184 | cd ~/ardupilot/ArduCopter/ 185 | sim_vehicle.py -j4 -f Gazebo --console 186 | ``` 187 | 188 | In another Terminal (Terminal 2), run Gazebo: 189 | ``` 190 | gazebo --verbose ~/ardupilot_gazebo/gazebo_worlds/iris_irlock_demo.world 191 | ``` 192 | 193 | Set parameters for sim in the same window after you run the `sim_vehicle.py script`. 194 | Do this by using command `param load ` 195 | Example params can be found in the `Controls-Other` repo: https://github.com/Texas-Aerial-Robotics/Controls-Other 196 | 197 | To load our parameters, clone our `Controls-Other` repository (Terminal 3): 198 | ``` 199 | cd ~ 200 | git clone https://github.com/Texas-Aerial-Robotics/Controls-Other 201 | ``` 202 | In the Ardupilot simulator Terminal (Terminal 1) run: 203 | ``` 204 | param load ../../Controls-Other/Params/Sim_master.param 205 | ``` 206 | Play around, then close by "Control+C" in the Terminal windows. 207 | 208 | ## 9. Install Roomba Drone Simulator 209 | 210 | In order to use the Ardupilot simulator with the Roombas - as well as other plugins such as cameras - you need to add ROS packages to Gazebo. 211 | Install the following dependencies: 212 | ``` 213 | sudo apt install ros-melodic-gazebo-ros ros-melodic-gazebo-plugins 214 | ``` 215 | 216 | Clone in Gazebo-Ros in your catkin_ws and the Computations repo in your home directory: 217 | ``` 218 | cd ~/catkin_ws/src 219 | git clone https://github.com/Texas-Aerial-Robotics/Gazebo-Ros.git 220 | cd ~ 221 | git clone https://github.com/Texas-Aerial-Robotics/Computations.git 222 | ``` 223 | 224 | Add the following to your `~/.bashrc` to use the Roomba plugins: 225 | ``` 226 | export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/Computations/roomba_host/models 227 | export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/Computations/roomba_host/build 228 | ``` 229 | 230 | Build the ROS packages you just downloaded: 231 | ``` 232 | cd ~/catkin_ws/ 233 | catkin build 234 | ``` 235 | 236 | Reload `~/.bashrc` so the Terminal knows where the packages are located: 237 | ``` 238 | source ~/.bashrc 239 | ``` 240 | 241 | ### Run a sim with roslaunch 242 | In one Terminal (Terminal 1): 243 | ``` 244 | roslaunch gazebo_ros_sim droneOnly.launch 245 | ``` 246 | In another Terminal (Terminal 2), run the following command to start and connect the Ardupilot simulator to Gazebo: 247 | ``` 248 | cd ~/ardupilot/ArduCopter/ && sim_vehicle.py -f gazebo-iris --console -I0 249 | ``` 250 | Play around, then close by "Control+C" in the Terminal windows. 251 | 252 | ### To view camera plugin 253 | Install image view: 254 | ``` 255 | sudo apt install ros-melodic-image-view 256 | ``` 257 | 258 | To see camera view: 259 | ``` 260 | rosrun image_view image_view image:=/webcam/image_raw 261 | ``` 262 | 263 | ## 10. Run Mavros Scripts from Controls-Ros package 264 | 265 | Launch the sim via roslaunch (Terminal 1): 266 | ``` 267 | roslaunch gazebo_ros_sim droneOnly.launch 268 | ``` 269 | In another Terminal (Terminal 2) run the following command to start and connect the Ardupilot sim to Gazebo: 270 | ``` 271 | cd ~/ardupilot/ArduCopter/ && sim_vehicle.py -f gazebo-iris --console -I0 272 | ``` 273 | In another Terminal (Terminal 3) run: 274 | ``` 275 | cd ~/Controls-Other/Launch/ 276 | roslaunch apm.launch 277 | ``` 278 | Be sure to wait for the drone to go throught the full initialization process. The drone will be ready to fly once the console reads Optical Flow now aiding and origin set to GPS. For the competition we will not use GPS to home the drone, but for introduction sake let the drone use GPS 279 | 280 | In another Terminal (Terminal 4) run: 281 | ``` 282 | rosrun flight_pkg staple 283 | ``` 284 | 285 | Then in MavProxy (Terminal 2) switch the mode into `guided`: 286 | ``` 287 | mode GUIDED 288 | ``` 289 | 290 | you did it! 🎉 291 | 292 | --- 293 | 294 | ## Troubleshooting and Common Problems: 295 | ### During Catkin Build: 296 | #### Complains about "GeographicLib" 297 | ``` 298 | cd ~/catkin_ws/src/mavros/mavros/scripts 299 | sudo ./install_geographiclib_datasets.sh 300 | ``` 301 | 302 | #### Complains about "Geometry Msgs" 303 | ``` 304 | sudo apt install ros-melodic-geometry-msgs 305 | ``` 306 | 307 | --- 308 | 309 | References: 310 | - https://dev.px4.io/en/ros/mavros_installation.html#source-installation 311 | - http://wiki.ros.org/catkin/Tutorials/create_a_workspace 312 | - https://github.com/ArduPilot/ardupilot_wiki/issues/1001 313 | - https://github.com/AS4SR/general_info/wiki/ArduPilot:-Instructions-to-set-up-and-run-an-autopilot-using-SITL-and-Gazebo-simulator 314 | -------------------------------------------------------------------------------- /cfg/contolParams.yaml: -------------------------------------------------------------------------------- 1 | safety_limits: 2 | xlim: 3 | min: -20.5 4 | max: 19.5 5 | ylim: 6 | min: -20.5 7 | max: 19.5 8 | zlim: 9 | min: 0.0 10 | max: 3.0 -------------------------------------------------------------------------------- /init.sh: -------------------------------------------------------------------------------- 1 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 2 | sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 3 | sudo apt update 4 | sudo apt install ros-kinetic-desktop 5 | sudo rosdep init 6 | rosdep update 7 | echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 8 | source ~/.bashrc 9 | sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential 10 | sudo apt install python-wstool python-rosinstall-generator python-catkin-tools 11 | mkdir -p ~/catkin_ws/src 12 | cd ~/catkin_ws 13 | catkin init 14 | cd ~/catkin_ws 15 | wstool init ~/catkin_ws/src 16 | rosinstall_generator --upstream mavros | tee /tmp/mavros.rosinstall 17 | rosinstall_generator mavlink | tee -a /tmp/mavros.rosinstall 18 | wstool merge -t src /tmp/mavros.rosinstall 19 | wstool update -t src 20 | rosdep install --from-paths src --ignore-src --rosdistro `echo $ROS_DISTRO` -y 21 | catkin build 22 | echo "source ~/catkin_ws/devel/setup.bash " >> ~/.bashrc 23 | source ~/.bashrc 24 | cd ~/catkin_ws/src 25 | git clone https://github.com/Texas-Aerial-Robotics/Controls-ROS.git 26 | cd ~/catkin_ws 27 | catkin build 28 | cd ~ 29 | git clone https://github.com/Texas-Aerial-Robotics/ardupilot.git 30 | cd ardupilot 31 | git submodule update --init --recursive 32 | sudo apt install python-matplotlib python-serial python-wxgtk3.0 python-wxtools python-lxml 33 | sudo apt install python-scipy python-opencv ccache gawk git python-pip python-pexpect 34 | sudo pip2 install future pymavlink MAVProxy 35 | echo "export PATH=$PATH:$HOME/ardupilot/Tools/autotest" >> ~/.bashrc 36 | echo "export PATH=/usr/lib/ccache:$PATH" >> ~/.bashrc 37 | source ~/.bashrc 38 | cd ~/ardupilot/ArduCopter 39 | sim_vehicle.py -w & 40 | sleep 20 41 | kill %1 42 | sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 43 | wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - 44 | sudo apt-get update 45 | sudo apt-get install gazebo7 libgazebo7-dev 46 | sudo apt install ros-kinetic-gazebo-ros ros-kinetic-gazebo-plugins 47 | cd ~ 48 | git clone https://github.com/SwiftGust/ardupilot_gazebo 49 | cd ardupilot_gazebo 50 | mkdir build 51 | cd build 52 | cmake .. 53 | make -j4 54 | sudo make install 55 | echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/gazebo_models' >> ~/.bashrc 56 | source ~/.bashrc 57 | hg clone https://bitbucket.org/osrf/gazebo_models ~/gazebo_ws/gazebo_models 58 | cd ~/gazebo_ws/gazebo_models 59 | hg checkout zephyr_demos 60 | echo 'export GAZEBO_MODEL_PATH=~/gazebo_ws/gazebo_models' >> ~/.bashrc 61 | source ~/.bashrc 62 | cd ~ 63 | git clone https://github.com/Texas-Aerial-Robotics/Controls-Other 64 | -------------------------------------------------------------------------------- /launch/apm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/apmsitl.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 | -------------------------------------------------------------------------------- /launch/follow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/followScan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/offset.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 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /launch/px4flow.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 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flight_pkg 4 | 0.0.0 5 | The flight_pkg package 6 | 7 | 8 | 9 | 10 | Texas Aerial Robotics 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | https://github.com/Texas-Aerial-Robotics/Controls-ROS 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | mavros 44 | mavros 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /src/darknetSub.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "std_msgs/Int8.h" 4 | #include "darknet_ros_msgs/BoundingBox.h" 5 | #include "darknet_ros_msgs/BoundingBoxes.h" 6 | #include 7 | #include 8 | #include 9 | /** 10 | * This tutorial demonstrates simple receipt of messages over the ROS system. 11 | */ 12 | 13 | void chatterCallback(const std_msgs::Int8::ConstPtr& msg) 14 | { 15 | int num = msg->data; 16 | //ROS_INFO("%d objects found", num); 17 | } 18 | void centerPoint(const darknet_ros_msgs::BoundingBoxes::ConstPtr& msgBox) 19 | { 20 | darknet_ros_msgs::BoundingBoxes boxesFound; 21 | darknet_ros_msgs::BoundingBox objectBounds; 22 | boxesFound = *msgBox; 23 | for(int i=0; i < boxesFound.boundingBoxes.size(); i++) 24 | { 25 | objectBounds = boxesFound.boundingBoxes[i]; 26 | int xCenter = (objectBounds.xmax + objectBounds.xmin)/2; 27 | int yCenter = (objectBounds.ymax + objectBounds.ymin)/2; 28 | std::string objectType = objectBounds.Class; 29 | ROS_INFO("%s found x: %d y: %d", objectType.c_str(), xCenter, yCenter); 30 | } 31 | } 32 | int main(int argc, char **argv) 33 | { 34 | /** 35 | * The ros::init() function needs to see argc and argv so that it can perform 36 | * any ROS arguments and name remapping that were provided at the command line. 37 | * For programmatic remappings you can use a different version of init() which takes 38 | * remappings directly, but for most command-line programs, passing argc and argv is 39 | * the easiest way to do it. The third argument to init() is the name of the node. 40 | * 41 | * You must call one of the versions of ros::init() before using any other 42 | * part of the ROS system. 43 | */ 44 | ros::init(argc, argv, "listener"); 45 | 46 | /** 47 | * NodeHandle is the main access point to communications with the ROS system. 48 | * The first NodeHandle constructed will fully initialize this node, and the last 49 | * NodeHandle destructed will close down the node. 50 | */ 51 | ros::NodeHandle n; 52 | /** 53 | * The subscribe() call is how you tell ROS that you want to receive messages 54 | * on a given topic. This invokes a call to the ROS 55 | * master node, which keeps a registry of who is publishing and who 56 | * is subscribing. Messages are passed to a callback function, here 57 | * called chatterCallback. subscribe() returns a Subscriber object that you 58 | * must hold on to until you want to unsubscribe. When all copies of the Subscriber 59 | * object go out of scope, this callback will automatically be unsubscribed from 60 | * this topic. 61 | * 62 | * The second parameter to the subscribe() function is the size of the message 63 | * queue. If messages are arriving faster than they are being processed, this 64 | * is the number of messages that will be buffered up before beginning to throw 65 | * away the oldest ones. 66 | */ 67 | ros::Subscriber sub2 = n.subscribe("/darknet_ros/bounding_boxes",1 ,centerPoint); 68 | ros::Subscriber sub = n.subscribe("/darknet_ros/found_object", 1, chatterCallback); 69 | 70 | /** 71 | * ros::spin() will enter a loop, pumping callbacks. With this version, all 72 | * callbacks will be called from within this thread (the main one). ros::spin() 73 | * will exit when Ctrl-C is pressed, or the node is shutdown by the master. 74 | */ 75 | ros::spin(); 76 | 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /src/examples/darknetSub.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "std_msgs/Int8.h" 4 | #include "darknet_ros_msgs/BoundingBox.h" 5 | #include "darknet_ros_msgs/BoundingBoxes.h" 6 | #include 7 | #include 8 | #include 9 | /** 10 | * This tutorial demonstrates simple receipt of messages over the ROS system. 11 | */ 12 | 13 | void chatterCallback(const std_msgs::Int8::ConstPtr& msg) 14 | { 15 | int num = msg->data; 16 | //ROS_INFO("%d objects found", num); 17 | } 18 | void centerPoint(const darknet_ros_msgs::BoundingBoxes::ConstPtr& msgBox) 19 | { 20 | darknet_ros_msgs::BoundingBoxes boxesFound; 21 | darknet_ros_msgs::BoundingBox objectBounds; 22 | boxesFound = *msgBox; 23 | for(int i=0; i < boxesFound.boundingBoxes.size(); i++) 24 | { 25 | objectBounds = boxesFound.boundingBoxes[i]; 26 | int xCenter = (objectBounds.xmax + objectBounds.xmin)/2; 27 | int yCenter = (objectBounds.ymax + objectBounds.ymin)/2; 28 | std::string objectType = objectBounds.Class; 29 | ROS_INFO("%s found x: %d y: %d", objectType.c_str(), xCenter, yCenter); 30 | } 31 | } 32 | int main(int argc, char **argv) 33 | { 34 | /** 35 | * The ros::init() function needs to see argc and argv so that it can perform 36 | * any ROS arguments and name remapping that were provided at the command line. 37 | * For programmatic remappings you can use a different version of init() which takes 38 | * remappings directly, but for most command-line programs, passing argc and argv is 39 | * the easiest way to do it. The third argument to init() is the name of the node. 40 | * 41 | * You must call one of the versions of ros::init() before using any other 42 | * part of the ROS system. 43 | */ 44 | ros::init(argc, argv, "listener"); 45 | 46 | /** 47 | * NodeHandle is the main access point to communications with the ROS system. 48 | * The first NodeHandle constructed will fully initialize this node, and the last 49 | * NodeHandle destructed will close down the node. 50 | */ 51 | ros::NodeHandle n; 52 | /** 53 | * The subscribe() call is how you tell ROS that you want to receive messages 54 | * on a given topic. This invokes a call to the ROS 55 | * master node, which keeps a registry of who is publishing and who 56 | * is subscribing. Messages are passed to a callback function, here 57 | * called chatterCallback. subscribe() returns a Subscriber object that you 58 | * must hold on to until you want to unsubscribe. When all copies of the Subscriber 59 | * object go out of scope, this callback will automatically be unsubscribed from 60 | * this topic. 61 | * 62 | * The second parameter to the subscribe() function is the size of the message 63 | * queue. If messages are arriving faster than they are being processed, this 64 | * is the number of messages that will be buffered up before beginning to throw 65 | * away the oldest ones. 66 | */ 67 | ros::Subscriber sub2 = n.subscribe("/darknet_ros/bounding_boxes",1 ,centerPoint); 68 | ros::Subscriber sub = n.subscribe("/darknet_ros/found_object", 1, chatterCallback); 69 | 70 | /** 71 | * ros::spin() will enter a loop, pumping callbacks. With this version, all 72 | * callbacks will be called from within this thread (the main one). ros::spin() 73 | * will exit when Ctrl-C is pressed, or the node is shutdown by the master. 74 | */ 75 | ros::spin(); 76 | 77 | 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /src/examples/frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @file frame.cpp 3 | * @Basic framework file 4 | */ 5 | 6 | #include //Include the ROS basics, it allows us to make our nodes and similar stuff 7 | 8 | //Include the msg types we need for the program 9 | #include //Msg for arming_client ServiceClient 10 | #include //Msg for set_mode_client ServiceClient 11 | #include //Msg for state_sub Subscriber 12 | #include //Msg for att_pub Publisher 13 | #include //Msg for rng_sub Subscriber 14 | 15 | //Set some global messages for our subscribers to store data in 16 | mavros_msgs::State current_state; 17 | sensor_msgs::Range rngfnd; 18 | 19 | //Create a function that is triggered by our state_sub subscriber 20 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 21 | //Store our state data in our global variable for access in other places 22 | current_state = *msg; 23 | } 24 | 25 | //Create a function that is triggered by our rng_sub subscriber 26 | void rng_cb(const sensor_msgs::Range::ConstPtr& msg){ 27 | //Store our range data in our global variable 28 | rngfnd = *msg; 29 | } 30 | 31 | //Start of our main program 32 | int main(int argc, char **argv) 33 | { 34 | //These lines create a node with the name offb_node that can be access from the handle nh 35 | ros::init(argc, argv, "offb_node"); 36 | ros::NodeHandle nh; 37 | 38 | //These are our subscribers, they listen to the given ROS topics and when they hear a 39 | // message on their topic they run their respective function 40 | ros::Subscriber state_sub = nh.subscribe("mavros/state", 1, state_cb); 41 | ros::Subscriber rng_sub = nh.subscribe("mavros/rangefinder/rangefinder",1,rng_cb); 42 | 43 | //These are our publishers, they are called as functions and publish messages to a specific topic 44 | ros::Publisher att_pub = nh.advertise("mavros/setpoint_raw/attitude", 10); 45 | 46 | //These are our Service Clients, they operate on call and response for important things such as arming the quad 47 | ros::ServiceClient arming_client = nh.serviceClient("mavros/cmd/arming"); 48 | ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 49 | 50 | //The publishing rate of setpoints, must be higher than 2Hz 51 | ros::Rate rate(20.0); 52 | 53 | //Wait for the quad to come online and connect 54 | while(ros::ok() && current_state.connected){ 55 | //This command refreshes all publishers, subscribers, and other callbacks 56 | ros::spinOnce(); 57 | rate.sleep(); 58 | } 59 | 60 | //This is an example of defining an attitude message 61 | mavros_msgs::AttitudeTarget att; 62 | att.orientation.x=0; 63 | att.orientation.y=0; 64 | att.orientation.z=0; 65 | att.orientation.w=0; 66 | att.body_rate.x=0; 67 | att.body_rate.y=0; 68 | att.body_rate.z=0; 69 | att.thrust=.55; 70 | 71 | 72 | //Send a few setpoints before we start up 73 | for(int i = 100; ros::ok() && i > 0; --i){ 74 | //This is an example of publishing a message 75 | att_pub.publish(att); 76 | ros::spinOnce(); 77 | rate.sleep(); 78 | } 79 | 80 | //Here we have our Mode message definitions 81 | mavros_msgs::SetMode offb_set_mode; 82 | offb_set_mode.request.custom_mode = "GUIDED_NOGPS"; 83 | 84 | mavros_msgs::SetMode alt_set_mode; 85 | alt_set_mode.request.custom_mode = "LAND"; 86 | 87 | //Here we define our arm command msg 88 | mavros_msgs::CommandBool arm_cmd; 89 | arm_cmd.request.value = true; 90 | 91 | //This is intialized so we don't overload the FC 92 | ros::Time last_request = ros::Time::now(); 93 | 94 | //The meat of the program, when everything else has been initalized we move on to here 95 | while(ros::ok()){ 96 | //Here is where we check to see if we're supposed to switch into offboard mode 97 | if( current_state.mode == "ACRO" && (ros::Time::now() - last_request > ros::Duration(5.0))){ 98 | //Since this is a service client call we get either a true or a false in return depending on whether it succeeded 99 | if( set_mode_client.call(offb_set_mode)){ 100 | ROS_INFO("Offboard enabled"); 101 | } 102 | //reset our last request time 103 | last_request = ros::Time::now(); 104 | } else { 105 | //If we've switched into offboard mode and we aren't armed we try to arm 106 | if( !current_state.armed && current_state.mode == "GUIDED_NOGPS" && (ros::Time::now() - last_request > ros::Duration(5.0))){ 107 | if( arming_client.call(arm_cmd) && arm_cmd.response.success){ 108 | ROS_INFO("Vehicle armed"); 109 | } 110 | last_request = ros::Time::now(); 111 | } 112 | } 113 | //This is where we continue to send setpoints, realistically this should be 114 | // more complex but because we only took off and landed we just kept sending the same waypoint 115 | att_pub.publish(att); 116 | 117 | //If we're in offboard mode and we've made it above our desired 118 | // altitude as reported by the lidar we move to landing mode 119 | if(rngfnd.range>1.5 && current_state.mode=="GUIDED_NOGPS"){ 120 | if(set_mode_client.call(alt_set_mode)){ 121 | ROS_INFO("land"); 122 | } 123 | } 124 | 125 | //Refresh our subs and pubs every time so we have updated data 126 | ros::spinOnce(); 127 | rate.sleep(); 128 | } 129 | 130 | return 0; 131 | } -------------------------------------------------------------------------------- /src/follow.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "std_msgs/Float64.h" 6 | #include "std_msgs/String.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | 21 | //Set global variables 22 | mavros_msgs::State current_state; 23 | nav_msgs::Odometry current_pose; 24 | geometry_msgs::PoseStamped waypoint; 25 | std_msgs::Float64 current_heading; 26 | std_msgs::String MODE; 27 | float GYM_OFFSET; 28 | float x_min; 29 | float x_max; 30 | float y_min; 31 | float y_max; 32 | float z_min; 33 | float z_max; 34 | 35 | //get armed state 36 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 37 | { 38 | current_state = *msg; 39 | } 40 | //get current position of drone 41 | void pose_cb(const nav_msgs::Odometry::ConstPtr& msg) 42 | { 43 | current_pose = *msg; 44 | ROS_INFO("x: %f y: %f x: %f", current_pose.pose.pose.position.x, current_pose.pose.pose.position.y, current_pose.pose.pose.position.z); 45 | } 46 | //get compass heading 47 | void heading_cb(const std_msgs::Float64::ConstPtr& msg) 48 | { 49 | current_heading = *msg; 50 | //ROS_INFO("current heading: %f", current_heading.data); 51 | } 52 | void mode_cb(const std_msgs::String::ConstPtr& msg) 53 | { 54 | MODE = *msg; 55 | ROS_INFO("current mode: %s", MODE.data.c_str()); 56 | } 57 | //set orientation of the drone (drone should always be level) 58 | void setHeading(float heading) 59 | { 60 | heading = -heading + 90 - GYM_OFFSET; 61 | float yaw = heading*(M_PI/180); 62 | float pitch = 0; 63 | float roll = 0; 64 | 65 | float cy = cos(yaw * 0.5); 66 | float sy = sin(yaw * 0.5); 67 | float cr = cos(roll * 0.5); 68 | float sr = sin(roll * 0.5); 69 | float cp = cos(pitch * 0.5); 70 | float sp = sin(pitch * 0.5); 71 | 72 | float qw = cy * cr * cp + sy * sr * sp; 73 | float qx = cy * sr * cp - sy * cr * sp; 74 | float qy = cy * cr * sp + sy * sr * cp; 75 | float qz = sy * cr * cp - cy * sr * sp; 76 | 77 | waypoint.pose.orientation.w = qw; 78 | waypoint.pose.orientation.x = qx; 79 | waypoint.pose.orientation.y = qy; 80 | waypoint.pose.orientation.z = qz; 81 | 82 | } 83 | // set position to fly to in the gym frame 84 | void setDestination(float x, float y, float z) 85 | { 86 | float deg2rad = (M_PI/180); 87 | float X = x*cos(-GYM_OFFSET*deg2rad) - y*sin(-GYM_OFFSET*deg2rad); 88 | float Y = x*sin(-GYM_OFFSET*deg2rad) + y*cos(-GYM_OFFSET*deg2rad); 89 | float Z = z; 90 | waypoint.pose.position.x = X; 91 | waypoint.pose.position.y = Y; 92 | waypoint.pose.position.z = Z; 93 | ROS_INFO("Destination set to x: %f y: %f z %f", X, Y, Z); 94 | } 95 | 96 | //initialize ROS 97 | void init_ros() 98 | { 99 | ROS_INFO("INITIALIZING ROS"); 100 | for(int i=0; i<100; i++) 101 | { 102 | ros::spinOnce(); 103 | ros::Duration(0.01).sleep(); 104 | } 105 | } 106 | 107 | // update waypoint from strat but check inputs 108 | void waypoint_update(const geometry_msgs::PoseStamped::ConstPtr& msg) 109 | { 110 | geometry_msgs::PoseStamped waypoint_eval; 111 | waypoint_eval = *msg; 112 | float way_x = waypoint_eval.pose.position.x; 113 | float way_y = waypoint_eval.pose.position.y; 114 | float way_z = waypoint_eval.pose.position.z; 115 | 116 | if (way_z < z_max && way_z > z_min && way_x < x_max && way_x > x_min && way_y < y_max && way_y > y_min) 117 | { 118 | waypoint.pose.position.x = way_x; 119 | waypoint.pose.position.y = way_y; 120 | waypoint.pose.position.z = way_z; 121 | setDestination(way_x,way_y,way_z); 122 | } 123 | else 124 | { 125 | ROS_INFO("The waypoint passed is out of bounds at x,y,z = %f,%f,%f",way_x,way_y,way_z); 126 | } 127 | } 128 | 129 | int main(int argc, char** argv) 130 | { 131 | ros::init(argc, argv, "offb_node"); 132 | ros::NodeHandle controlnode; 133 | 134 | ros::param::get("/contols_params/safety_limits/xlim/min", x_min); 135 | ros::param::get("/contols_params/safety_limits/xlim/max", x_max); 136 | ros::param::get("/contols_params/safety_limits/ylim/min", y_min); 137 | ros::param::get("/contols_params/safety_limits/ylim/max", y_max); 138 | ros::param::get("/contols_params/safety_limits/xlim/min", z_min); 139 | ros::param::get("/contols_params/safety_limits/xlim/max", z_max); 140 | 141 | ros::Rate rate(5.0); 142 | ros::Subscriber state_sub = controlnode.subscribe("mavros/state", 10, state_cb); 143 | ros::Publisher set_vel_pub = controlnode.advertise("mavros/setpoint_raw/local", 10); 144 | ros::Publisher local_pos_pub = controlnode.advertise("mavros/setpoint_position/local", 10); 145 | ros::Publisher gym_offset_pub = controlnode.advertise("gymOffset", 1); 146 | ros::Subscriber currentPos = controlnode.subscribe("mavros/global_position/local", 10, pose_cb); 147 | ros::Subscriber currentHeading = controlnode.subscribe("mavros/global_position/compass_hdg", 10, heading_cb); 148 | ros::Subscriber waypointSubscrib = controlnode.subscribe("waypoint", 10, waypoint_update); 149 | ros::Subscriber mode_sub = controlnode.subscribe("mode", 1, mode_cb); 150 | ros::ServiceClient arming_client = controlnode.serviceClient("mavros/cmd/arming"); 151 | ros::ServiceClient takeoff_client = controlnode.serviceClient("/mavros/cmd/takeoff"); 152 | 153 | 154 | 155 | // wait for FCU connection 156 | while (ros::ok() && !current_state.connected) 157 | { 158 | ros::spinOnce(); 159 | rate.sleep(); 160 | } 161 | ROS_INFO("Connected to FCU"); 162 | // wait for mode to be set to guided 163 | while (current_state.mode != "GUIDED") 164 | { 165 | ros::spinOnce(); 166 | rate.sleep(); 167 | } 168 | ROS_INFO("Mode set to GUIDED"); 169 | 170 | //set the orientation of the gym 171 | GYM_OFFSET = 0; 172 | for (int i = 1; i <= 30; ++i) { 173 | ros::spinOnce(); 174 | ros::Duration(0.1).sleep(); 175 | GYM_OFFSET += current_heading.data; 176 | ROS_INFO("current heading%d: %f", i, GYM_OFFSET/i); 177 | } 178 | GYM_OFFSET /= 30; 179 | ROS_INFO("the N' axis is facing: %f", GYM_OFFSET); 180 | cout << GYM_OFFSET << "\n" << endl; 181 | std_msgs::Float64 gymOffset; 182 | gymOffset.data = GYM_OFFSET; 183 | gym_offset_pub.publish(gymOffset); 184 | // arming 185 | mavros_msgs::CommandBool arm_request; 186 | arm_request.request.value = true; 187 | while (!current_state.armed && !arm_request.response.success) 188 | { 189 | ros::Duration(.1).sleep(); 190 | arming_client.call(arm_request); 191 | } 192 | ROS_INFO("ARM sent %d", arm_request.response.success); 193 | 194 | 195 | //request takeoff 196 | mavros_msgs::CommandTOL takeoff_request; 197 | takeoff_request.request.altitude = 1.5; 198 | 199 | while (!takeoff_request.response.success) 200 | { 201 | ros::Duration(.1).sleep(); 202 | takeoff_client.call(takeoff_request); 203 | } 204 | ROS_INFO("Takeoff initialized"); 205 | sleep(10); 206 | setDestination(0,0,1.5); 207 | //move foreward 208 | setHeading(0); 209 | float tollorance = .35; 210 | while(ros::ok()) 211 | { 212 | ros::spinOnce(); 213 | rate.sleep(); 214 | float deltaX = abs(waypoint.pose.position.x - current_pose.pose.pose.position.x); 215 | float deltaY = abs(waypoint.pose.position.y - current_pose.pose.pose.position.y); 216 | float deltaZ = abs(waypoint.pose.position.z - current_pose.pose.pose.position.z); 217 | //cout << " dx " << deltaX << " dy " << deltaY << " dz " << deltaZ << endl; 218 | float dMag = sqrt( pow(deltaX, 2) + pow(deltaY, 2) + pow(deltaZ, 2) ); 219 | cout << dMag << endl; 220 | if( dMag < tollorance) 221 | { 222 | if(MODE.data == "GOTO") 223 | { 224 | ros::ServiceClient land_client = controlnode.serviceClient("/mavros/cmd/land"); 225 | mavros_msgs::CommandTOL srv_land; 226 | if (land_client.call(srv_land) && srv_land.response.success) 227 | ROS_INFO("land sent %d", srv_land.response.success); 228 | else 229 | { 230 | ROS_ERROR("Landing failed"); 231 | ros::shutdown(); 232 | return -1; 233 | } 234 | 235 | while (ros::ok()) 236 | { 237 | ros::spinOnce(); 238 | rate.sleep(); 239 | } 240 | } 241 | continue; 242 | }else{ 243 | local_pos_pub.publish(waypoint); 244 | } 245 | 246 | //gym_offset_pub.publish(GYM_OFFSET); 247 | 248 | 249 | } 250 | return 0; 251 | } 252 | -------------------------------------------------------------------------------- /src/followWithScan.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "std_msgs/Float64.h" 6 | #include "std_msgs/String.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | 22 | //Set global variables 23 | mavros_msgs::State current_state; 24 | nav_msgs::Odometry current_pose; 25 | geometry_msgs::PoseStamped waypoint; 26 | sensor_msgs::LaserScan current_2D_scan; 27 | 28 | std_msgs::String MODE; 29 | float GYM_OFFSET; 30 | float x_min; 31 | float x_max; 32 | float y_min; 33 | float y_max; 34 | float z_min; 35 | float z_max; 36 | float current_heading; 37 | double RUN_START_TIME = 0; 38 | bool currentlyAvoiding = false; 39 | bool lidarPresent = true; 40 | 41 | //get armed state 42 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 43 | { 44 | current_state = *msg; 45 | } 46 | //get current position of drone 47 | void pose_cb(const nav_msgs::Odometry::ConstPtr& msg) 48 | { 49 | current_pose = *msg; 50 | float q0 = current_pose.pose.pose.orientation.w; 51 | float q1 = current_pose.pose.pose.orientation.x; 52 | float q2 = current_pose.pose.pose.orientation.y; 53 | float q3 = current_pose.pose.pose.orientation.z; 54 | float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) ); 55 | current_heading = -psi*(180/M_PI) + 90; 56 | ROS_INFO("Current Heading %f ", current_heading); 57 | // ROS_INFO("x: %f y: %f z: %f", current_pose.pose.pose.position.x, current_pose.pose.pose.position.y, current_pose.pose.pose.position.z); 58 | } 59 | void mode_cb(const std_msgs::String::ConstPtr& msg) 60 | { 61 | MODE = *msg; 62 | //ROS_INFO("current mode: %s", MODE.data.c_str()); 63 | } 64 | void scan_cb(const sensor_msgs::LaserScan::ConstPtr& msg) 65 | { 66 | current_2D_scan = *msg; 67 | } 68 | //set orientation of the drone (drone should always be level) 69 | void setHeading(float heading) 70 | { 71 | heading = -heading + 90 - GYM_OFFSET; 72 | float yaw = heading*(M_PI/180); 73 | float pitch = 0; 74 | float roll = 0; 75 | 76 | float cy = cos(yaw * 0.5); 77 | float sy = sin(yaw * 0.5); 78 | float cr = cos(roll * 0.5); 79 | float sr = sin(roll * 0.5); 80 | float cp = cos(pitch * 0.5); 81 | float sp = sin(pitch * 0.5); 82 | 83 | float qw = cy * cr * cp + sy * sr * sp; 84 | float qx = cy * sr * cp - sy * cr * sp; 85 | float qy = cy * cr * sp + sy * sr * cp; 86 | float qz = sy * cr * cp - cy * sr * sp; 87 | 88 | waypoint.pose.orientation.w = qw; 89 | waypoint.pose.orientation.x = qx; 90 | waypoint.pose.orientation.y = qy; 91 | waypoint.pose.orientation.z = qz; 92 | } 93 | void setHeading_cb(const std_msgs::Float64::ConstPtr& msg) 94 | { 95 | std_msgs::Float64 set_heading = *msg; 96 | setHeading(set_heading.data); 97 | // ROS_INFO("current heading: %f", current_heading.data); 98 | } 99 | // set position to fly to in the gym frame 100 | void setDestination(float x, float y, float z) 101 | { 102 | float deg2rad = (M_PI/180); 103 | float X = x*cos(-GYM_OFFSET*deg2rad) - y*sin(-GYM_OFFSET*deg2rad); 104 | float Y = x*sin(-GYM_OFFSET*deg2rad) + y*cos(-GYM_OFFSET*deg2rad); 105 | float Z = z; 106 | waypoint.pose.position.x = X; 107 | waypoint.pose.position.y = Y; 108 | waypoint.pose.position.z = Z; 109 | // ROS_INFO("Destination set to x: %f y: %f z: %f", X, Y, Z); 110 | cout << "Destination set to x: " << X << " y: " << Y << " z: " << Z << endl; 111 | } 112 | 113 | //initialize ROS 114 | void init_ros() 115 | { 116 | ROS_INFO("INITIALIZING ROS"); 117 | for(int i=0; i<100; i++) 118 | { 119 | ros::spinOnce(); 120 | ros::Duration(0.01).sleep(); 121 | } 122 | } 123 | 124 | // update waypoint from strat but check inputs 125 | void waypoint_update(const geometry_msgs::PoseStamped::ConstPtr& msg) 126 | { 127 | geometry_msgs::PoseStamped waypoint_eval; 128 | waypoint_eval = *msg; 129 | float way_x = waypoint_eval.pose.position.x; 130 | float way_y = waypoint_eval.pose.position.y; 131 | float way_z = waypoint_eval.pose.position.z; 132 | 133 | if(!currentlyAvoiding) 134 | { 135 | if(way_z < z_max && way_z > z_min && way_x < x_max && way_x > x_min && way_y < y_max && way_y > y_min) 136 | { 137 | waypoint.pose.position.x = way_x; 138 | waypoint.pose.position.y = way_y; 139 | waypoint.pose.position.z = way_z; 140 | setDestination(way_x,way_y,way_z); 141 | } 142 | else 143 | { 144 | ROS_INFO("The waypoint passed is out of bounds at x,y,z = %f,%f,%f",way_x,way_y,way_z); 145 | } 146 | } 147 | else 148 | { 149 | ROS_INFO("Waypoint ignored. Obs avoidance in progress..."); 150 | } 151 | } 152 | 153 | int main(int argc, char** argv) 154 | { 155 | ros::init(argc, argv, "offb_node"); 156 | ros::NodeHandle controlnode; 157 | 158 | ros::param::get("/contols_params/safety_limits/xlim/min", x_min); 159 | ros::param::get("/contols_params/safety_limits/xlim/max", x_max); 160 | ros::param::get("/contols_params/safety_limits/ylim/min", y_min); 161 | ros::param::get("/contols_params/safety_limits/ylim/max", y_max); 162 | ros::param::get("/contols_params/safety_limits/xlim/min", z_min); 163 | ros::param::get("/contols_params/safety_limits/xlim/max", z_max); 164 | 165 | ros::Rate rate(5.0); 166 | ros::Publisher gym_offset_pub = controlnode.advertise("gymOffset", 1); 167 | ros::Publisher local_pos_pub = controlnode.advertise("mavros/setpoint_position/local", 10); 168 | ros::Publisher run_start_pub = controlnode.advertise("runStartTime", 1); 169 | ros::Subscriber collision_sub = controlnode.subscribe("/scan", 1, scan_cb); 170 | ros::Subscriber currentPos = controlnode.subscribe("mavros/global_position/local", 10, pose_cb); 171 | ros::Subscriber heading_pub = controlnode.subscribe("setHeading", 1, setHeading_cb); 172 | ros::Subscriber mode_sub = controlnode.subscribe("mode", 10, mode_cb); 173 | ros::Subscriber state_sub = controlnode.subscribe("mavros/state", 10, state_cb); 174 | ros::Subscriber waypointSubscrib = controlnode.subscribe("TARwaypoint", 10, waypoint_update); 175 | ros::ServiceClient arming_client = controlnode.serviceClient("mavros/cmd/arming"); 176 | ros::ServiceClient land_client = controlnode.serviceClient("/mavros/cmd/land"); 177 | ros::ServiceClient set_mode_client = controlnode.serviceClient("/mavros/set_mode"); 178 | ros::ServiceClient takeoff_client = controlnode.serviceClient("/mavros/cmd/takeoff"); 179 | 180 | // wait for FCU connection 181 | while (ros::ok() && !current_state.connected) 182 | { 183 | ros::spinOnce(); 184 | rate.sleep(); 185 | } 186 | ROS_INFO("Connected to FCU"); 187 | // wait for mode to be set to guided 188 | while (current_state.mode != "GUIDED") 189 | { 190 | ros::spinOnce(); 191 | rate.sleep(); 192 | } 193 | RUN_START_TIME = ros::Time::now().toSec(); 194 | 195 | // Publish mission start time 196 | std_msgs::Float64 runStartTime; 197 | runStartTime.data = RUN_START_TIME; 198 | run_start_pub.publish(runStartTime); 199 | ROS_INFO("Run start time %f", RUN_START_TIME); 200 | ROS_INFO("Mode set to GUIDED"); 201 | 202 | //set the orientation of the gym 203 | GYM_OFFSET = 0; 204 | for (int i = 1; i <= 30; ++i) { 205 | ros::spinOnce(); 206 | ros::Duration(0.1).sleep(); 207 | 208 | float q0 = current_pose.pose.pose.orientation.w; 209 | float q1 = current_pose.pose.pose.orientation.x; 210 | float q2 = current_pose.pose.pose.orientation.y; 211 | float q3 = current_pose.pose.pose.orientation.z; 212 | float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) ); // yaw 213 | 214 | GYM_OFFSET += psi*(180/M_PI); 215 | // ROS_INFO("current heading%d: %f", i, GYM_OFFSET/i); 216 | } 217 | GYM_OFFSET /= -30; 218 | GYM_OFFSET += 90; 219 | ROS_INFO("the N' axis is facing: %f", GYM_OFFSET); 220 | cout << GYM_OFFSET << "\n" << endl; 221 | std_msgs::Float64 gymOffset; 222 | gymOffset.data = GYM_OFFSET; 223 | gym_offset_pub.publish(gymOffset); 224 | 225 | // arming 226 | mavros_msgs::CommandBool arm_request; 227 | arm_request.request.value = true; 228 | while (!current_state.armed && !arm_request.response.success) 229 | { 230 | ros::Duration(.1).sleep(); 231 | arming_client.call(arm_request); 232 | } 233 | ROS_INFO("ARM sent %d", arm_request.response.success); 234 | 235 | 236 | //request takeoff 237 | mavros_msgs::CommandTOL takeoff_request; 238 | takeoff_request.request.altitude = 1.5; 239 | 240 | while (!takeoff_request.response.success) 241 | { 242 | ros::Duration(.1).sleep(); 243 | takeoff_client.call(takeoff_request); 244 | } 245 | ROS_INFO("Takeoff initialized"); 246 | sleep(10); 247 | 248 | setDestination(0,0,1.5); 249 | setHeading(0); 250 | 251 | // 2D spinning LIDAR params 252 | float tollorance = .35; 253 | float scanMinRange = .45; 254 | float scanMaxRange = 1.75; 255 | 256 | while(ros::ok()) 257 | { 258 | ros::spinOnce(); 259 | rate.sleep(); 260 | 261 | // 2D LIDAR obstacle avoidance 262 | int scanRayIndex = 2; 263 | int scanSize = current_2D_scan.ranges.size(); 264 | while(lidarPresent && ((scanRayIndex+2) < scanSize)) { 265 | scanRayIndex++; 266 | ros::spinOnce(); 267 | 268 | // check if 5 consecutive readings are in bounds we care about 269 | if((((current_2D_scan.ranges[scanRayIndex-2] < scanMaxRange) && (current_2D_scan.ranges[scanRayIndex-2] > scanMinRange)) && 270 | ((current_2D_scan.ranges[scanRayIndex-1] < scanMaxRange) && (current_2D_scan.ranges[scanRayIndex-1] > scanMinRange)) && 271 | ((current_2D_scan.ranges[scanRayIndex+0] < scanMaxRange) && (current_2D_scan.ranges[scanRayIndex+0] > scanMinRange)) && 272 | ((current_2D_scan.ranges[scanRayIndex+1] < scanMaxRange) && (current_2D_scan.ranges[scanRayIndex+1] > scanMinRange)) && 273 | ((current_2D_scan.ranges[scanRayIndex+2] < scanMaxRange) && (current_2D_scan.ranges[scanRayIndex+2] > scanMinRange)))) 274 | { 275 | break; // if so, stop scanning and move on 276 | } 277 | // Prints if we got a few detections but not a full 5 278 | if(current_2D_scan.ranges[scanRayIndex] < scanMaxRange && current_2D_scan.ranges[scanRayIndex] > scanMinRange) 279 | { 280 | ROS_INFO("Index[%d]: %f", scanRayIndex, current_2D_scan.ranges[scanRayIndex]); 281 | } 282 | if(!lidarPresent || (scanRayIndex+3) == scanSize) // scanRayIndex+2 because checking multiple rays and +1 because less than in while loop 283 | { 284 | currentlyAvoiding = false; 285 | } 286 | } 287 | if(lidarPresent && (scanRayIndex < scanSize && scanRayIndex > 0 && current_2D_scan.ranges[scanRayIndex] < scanMaxRange && current_2D_scan.ranges[scanRayIndex] > scanMinRange)) 288 | { 289 | double angle_of_obstacle_RAD = current_2D_scan.angle_increment*scanRayIndex; 290 | ROS_INFO("Obstacle sighted @%f (current_2D_scan.angle_increment: %f * scanRayIndex: %d)", angle_of_obstacle_RAD, current_2D_scan.angle_increment, scanRayIndex); 291 | ROS_INFO("-------------------SKRTT SKRTT-------------------"); 292 | 293 | //get current position in gym frame 294 | float deg2rad = (M_PI/180); 295 | float X = current_pose.pose.pose.position.x*cos(GYM_OFFSET*deg2rad) - current_pose.pose.pose.position.y*sin(GYM_OFFSET*deg2rad); 296 | float Y = current_pose.pose.pose.position.x*sin(GYM_OFFSET*deg2rad) + current_pose.pose.pose.position.y*cos(GYM_OFFSET*deg2rad); 297 | float Z = 1.5; 298 | ROS_INFO("x: %f y: %f z: %f", X, Y, Z); 299 | 300 | double radius = current_2D_scan.ranges[scanRayIndex]; 301 | double why = radius * sin(angle_of_obstacle_RAD); 302 | double ex = radius * cos(angle_of_obstacle_RAD); 303 | 304 | ROS_INFO("radius: %f, ex: %f, why: %f", radius, ex, why); 305 | 306 | //avoidance vector relative to the drone 307 | float avoidXdrone = (ex/radius)*(-1.328); 308 | float avoidYdrone = (why/radius)*(-1.328); 309 | 310 | //Transform to gym refernce frame 311 | float avoidXgym = avoidXdrone*cos(-(current_heading-GYM_OFFSET)*deg2rad) - avoidYdrone*sin(-(current_heading-GYM_OFFSET)*deg2rad); 312 | float avoidYgym = avoidXdrone*sin(-(current_heading-GYM_OFFSET)*deg2rad) + avoidYdrone*cos(-(current_heading-GYM_OFFSET)*deg2rad); 313 | 314 | ROS_INFO("goto x: %f, y: %f", avoidXgym, avoidYgym); 315 | setDestination((X + avoidXgym), (Y + avoidYgym), Z); 316 | currentlyAvoiding = true; 317 | 318 | rate.sleep(); 319 | rate.sleep(); 320 | rate.sleep(); 321 | } 322 | 323 | 324 | // LAND if StratNode says to or ten min run is done (600 sec == ten min) 325 | if((MODE.data == "LAND" || ((ros::Time::now().toSec()-RUN_START_TIME) > 590)) && !currentlyAvoiding) 326 | { 327 | mavros_msgs::CommandTOL srv_land; 328 | if(land_client.call(srv_land) && srv_land.response.success) 329 | ROS_INFO("land sent %d", srv_land.response.success); 330 | else 331 | { 332 | ROS_ERROR("Landing failed"); 333 | } 334 | } 335 | else if(MODE.data == "TAKEOFF") 336 | { 337 | while (current_state.armed && current_state.mode == "LAND") 338 | { 339 | rate.sleep(); 340 | ros::spinOnce(); 341 | } 342 | ros::Duration(5).sleep(); 343 | 344 | while (current_state.mode == "LAND") 345 | { 346 | //set flight mode to guided 347 | mavros_msgs::SetMode srv_setMode; 348 | srv_setMode.request.base_mode = 0; 349 | srv_setMode.request.custom_mode = "GUIDED"; 350 | if(set_mode_client.call(srv_setMode)){ 351 | ROS_INFO("setmode send ok"); 352 | }else{ 353 | ROS_ERROR("Failed SetMode"); 354 | return -1; 355 | } 356 | ros::Duration(.5).sleep(); 357 | ros::spinOnce(); 358 | } 359 | // arming 360 | mavros_msgs::CommandBool arm_request; 361 | arm_request.request.value = true; 362 | while (!current_state.armed && current_state.mode == "GUIDED") 363 | { 364 | arming_client.call(arm_request); 365 | ros::Duration(.1).sleep(); 366 | ros::spinOnce(); 367 | } 368 | ROS_INFO("ARM sent %d", arm_request.response.success); 369 | mavros_msgs::CommandTOL takeoff_request; 370 | takeoff_request.request.altitude = 1.5; 371 | 372 | while (!takeoff_request.response.success && current_state.mode == "GUIDED") 373 | { 374 | takeoff_client.call(takeoff_request); 375 | ros::Duration(.1).sleep(); 376 | ros::spinOnce(); 377 | } 378 | ROS_INFO("Takeoff sent"); 379 | ros::Duration(5).sleep(); 380 | } 381 | 382 | local_pos_pub.publish(waypoint); 383 | gym_offset_pub.publish(gymOffset); 384 | } 385 | return 0; 386 | } 387 | -------------------------------------------------------------------------------- /src/frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @file frame.cpp 3 | * @Basic framework file 4 | */ 5 | 6 | #include //Include the ROS basics, it allows us to make our nodes and similar stuff 7 | 8 | //Include the msg types we need for the program 9 | #include //Msg for arming_client ServiceClient 10 | #include //Msg for set_mode_client ServiceClient 11 | #include //Msg for state_sub Subscriber 12 | #include //Msg for att_pub Publisher 13 | #include //Msg for rng_sub Subscriber 14 | 15 | //Set some global messages for our subscribers to store data in 16 | mavros_msgs::State current_state; 17 | sensor_msgs::Range rngfnd; 18 | 19 | //Create a function that is triggered by our state_sub subscriber 20 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 21 | //Store our state data in our global variable for access in other places 22 | current_state = *msg; 23 | } 24 | 25 | //Create a function that is triggered by our rng_sub subscriber 26 | void rng_cb(const sensor_msgs::Range::ConstPtr& msg){ 27 | //Store our range data in our global variable 28 | rngfnd = *msg; 29 | } 30 | 31 | //Start of our main program 32 | int main(int argc, char **argv) 33 | { 34 | //These lines create a node with the name offb_node that can be access from the handle nh 35 | ros::init(argc, argv, "offb_node"); 36 | ros::NodeHandle nh; 37 | 38 | //These are our subscribers, they listen to the given ROS topics and when they hear a 39 | // message on their topic they run their respective function 40 | ros::Subscriber state_sub = nh.subscribe("mavros/state", 1, state_cb); 41 | ros::Subscriber rng_sub = nh.subscribe("mavros/rangefinder/rangefinder",1,rng_cb); 42 | 43 | //These are our publishers, they are called as functions and publish messages to a specific topic 44 | ros::Publisher att_pub = nh.advertise("mavros/setpoint_raw/attitude", 10); 45 | 46 | //These are our Service Clients, they operate on call and response for important things such as arming the quad 47 | ros::ServiceClient arming_client = nh.serviceClient("mavros/cmd/arming"); 48 | ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 49 | 50 | //The publishing rate of setpoints, must be higher than 2Hz 51 | ros::Rate rate(20.0); 52 | 53 | //Wait for the quad to come online and connect 54 | while(ros::ok() && current_state.connected){ 55 | //This command refreshes all publishers, subscribers, and other callbacks 56 | ros::spinOnce(); 57 | rate.sleep(); 58 | } 59 | 60 | //This is an example of defining an attitude message 61 | mavros_msgs::AttitudeTarget att; 62 | att.orientation.x=0; 63 | att.orientation.y=0; 64 | att.orientation.z=0; 65 | att.orientation.w=0; 66 | att.body_rate.x=0; 67 | att.body_rate.y=0; 68 | att.body_rate.z=0; 69 | att.thrust=.55; 70 | 71 | 72 | //Send a few setpoints before we start up 73 | for(int i = 100; ros::ok() && i > 0; --i){ 74 | //This is an example of publishing a message 75 | att_pub.publish(att); 76 | ros::spinOnce(); 77 | rate.sleep(); 78 | } 79 | 80 | //Here we have our Mode message definitions 81 | mavros_msgs::SetMode offb_set_mode; 82 | offb_set_mode.request.custom_mode = "GUIDED_NOGPS"; 83 | 84 | mavros_msgs::SetMode alt_set_mode; 85 | alt_set_mode.request.custom_mode = "LAND"; 86 | 87 | //Here we define our arm command msg 88 | mavros_msgs::CommandBool arm_cmd; 89 | arm_cmd.request.value = true; 90 | 91 | //This is intialized so we don't overload the FC 92 | ros::Time last_request = ros::Time::now(); 93 | 94 | //The meat of the program, when everything else has been initalized we move on to here 95 | while(ros::ok()){ 96 | //Here is where we check to see if we're supposed to switch into offboard mode 97 | if( current_state.mode == "ACRO" && (ros::Time::now() - last_request > ros::Duration(5.0))){ 98 | //Since this is a service client call we get either a true or a false in return depending on whether it succeeded 99 | if( set_mode_client.call(offb_set_mode)){ 100 | ROS_INFO("Offboard enabled"); 101 | } 102 | //reset our last request time 103 | last_request = ros::Time::now(); 104 | } else { 105 | //If we've switched into offboard mode and we aren't armed we try to arm 106 | if( !current_state.armed && current_state.mode == "GUIDED_NOGPS" && (ros::Time::now() - last_request > ros::Duration(5.0))){ 107 | if( arming_client.call(arm_cmd) && arm_cmd.response.success){ 108 | ROS_INFO("Vehicle armed"); 109 | } 110 | last_request = ros::Time::now(); 111 | } 112 | } 113 | //This is where we continue to send setpoints, realistically this should be 114 | // more complex but because we only took off and landed we just kept sending the same waypoint 115 | att_pub.publish(att); 116 | 117 | //If we're in offboard mode and we've made it above our desired 118 | // altitude as reported by the lidar we move to landing mode 119 | if(rngfnd.range>1.5 && current_state.mode=="GUIDED_NOGPS"){ 120 | if(set_mode_client.call(alt_set_mode)){ 121 | ROS_INFO("land"); 122 | } 123 | } 124 | 125 | //Refresh our subs and pubs every time so we have updated data 126 | ros::spinOnce(); 127 | rate.sleep(); 128 | } 129 | 130 | return 0; 131 | } -------------------------------------------------------------------------------- /src/more_logging.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | sensor_msgs::Range rng_msg; 14 | mavros_msgs::OpticalFlowRad opt_flow_msg; 15 | std_msgs::Float64 comp_hdg_msg; 16 | sensor_msgs::Imu imu_data_msg; 17 | sensor_msgs::Temperature imu_temp_msg; 18 | 19 | void opt_flow_cb(const mavros_msgs::OpticalFlowRad::ConstPtr& msg) 20 | { 21 | opt_flow_msg = *msg; 22 | } 23 | 24 | void comp_hdg_cb(const std_msgs::Float64::ConstPtr& msg) 25 | { 26 | comp_hdg_msg = *msg; 27 | } 28 | 29 | void imu_data_cb(const sensor_msgs::Imu::ConstPtr& msg) 30 | { 31 | imu_data_msg = *msg; 32 | } 33 | 34 | void imu_temp_cb(const sensor_msgs::Temperature::ConstPtr& msg) 35 | { 36 | imu_temp_msg = *msg; 37 | } 38 | 39 | void rng_cb(const sensor_msgs::Range::ConstPtr& msg) 40 | { 41 | rng_msg = *msg; 42 | } 43 | 44 | void init_timeHist(std::string file_name) 45 | { 46 | std::ofstream outTimeHist(file_name.c_str(),std::ios::app); 47 | 48 | outTimeHist << "Time(ms), Hdg(deg), Alt(m), Temp(C), optflow_intTime,optflow_intX,optflow_intY,optflow_intXgyro,"; 49 | outTimeHist << "optflow_intYgyro,optflow_intZgyro,optflow_quality,optflow_timeDel,optflow_dist,"; 50 | outTimeHist << "imu_orientX,imu_orientY,imu_orientZ,imu_orientW,imu_angularX(rad/sec),imu_angularY(rad/sec),imu_angularZ(rad/sec),"; 51 | outTimeHist << "imu_linAccelX(m/s^2),imu_linAccelY(m/s^2),imu_linAccelZ(m/s^2)" << std::endl; 52 | 53 | outTimeHist.close(); 54 | } 55 | 56 | const std::string currentDateTime() 57 | { 58 | time_t now = time(0); 59 | struct tm tstruct; 60 | char buf[80]; 61 | tstruct = *localtime(&now); 62 | strftime(buf,sizeof(buf),"%m_%d_%y-%H_%M", &tstruct); 63 | return buf; 64 | } 65 | 66 | int main(int argc, char **argv) 67 | { 68 | ros::init(argc, argv, "days_before_logs"); 69 | ros::NodeHandle dbl; 70 | 71 | ros::Subscriber rng = dbl.subscribe("mavros/rangefinder/rangefinder",1,rng_cb); 72 | ros::Subscriber opt_flow = dbl.subscribe("px4flow/px4flow/raw/optical_flow_rad",1,opt_flow_cb); 73 | ros::Subscriber comp_hdg = dbl.subscribe("mavros/global_position/compass_hdg",1,comp_hdg_cb); 74 | ros::Subscriber imu_data = dbl.subscribe("mavros/imu/data",1,imu_data_cb); 75 | ros::Subscriber imu_temp = dbl.subscribe("mavros/imu/temperature",1,imu_temp_cb); 76 | 77 | std::string date_file = "/sdCard/Logs/"+currentDateTime()+"_TimeHist.csv"; 78 | 79 | init_timeHist(date_file); 80 | 81 | struct timeval start,stop; 82 | gettimeofday(&start,NULL); 83 | long int ms_start = start.tv_sec * 1000 + start.tv_usec / 1000; 84 | while (ros::ok()) 85 | { 86 | ros::spinOnce(); 87 | std::ofstream out1Line("/sdCard/Logs/1Line.csv",std::ios::trunc); 88 | std::ofstream outTimeHist(date_file.c_str(),std::ios::app); 89 | 90 | 91 | float hdg = comp_hdg_msg.data; 92 | float rng = rng_msg.range; 93 | float temp = imu_temp_msg.temperature; 94 | 95 | 96 | int optflow_intTime = opt_flow_msg.integration_time_us; 97 | float optflow_intX = opt_flow_msg.integrated_x; 98 | float optflow_intY = opt_flow_msg.integrated_y; 99 | float optflow_intXgyro = opt_flow_msg.integrated_xgyro; 100 | float optflow_intYgyro = opt_flow_msg.integrated_ygyro; 101 | float optflow_intZgyro = opt_flow_msg.integrated_zgyro; 102 | float optflow_quality = opt_flow_msg.quality; 103 | float optflow_timeDel = opt_flow_msg.time_delta_distance_us; 104 | float optflow_dist = opt_flow_msg.distance; 105 | 106 | float imu_orientX = imu_data_msg.orientation.x; 107 | float imu_orientY = imu_data_msg.orientation.y; 108 | float imu_orientZ = imu_data_msg.orientation.z; 109 | float imu_orientW = imu_data_msg.orientation.w; 110 | float imu_angularX = imu_data_msg.angular_velocity.x; 111 | float imu_angularY = imu_data_msg.angular_velocity.y; 112 | float imu_angularZ = imu_data_msg.angular_velocity.z; 113 | float imu_linAccelX = imu_data_msg.linear_acceleration.x; 114 | float imu_linAccelY = imu_data_msg.linear_acceleration.y; 115 | float imu_linAccelZ = imu_data_msg.linear_acceleration.z; 116 | 117 | out1Line << hdg << "," << rng <<","<< temp << ","; 118 | out1Line << optflow_intTime << "," << optflow_intX << "," << optflow_intY << ","; 119 | out1Line << optflow_intXgyro << "," << optflow_intYgyro << "," << optflow_intZgyro << ","; 120 | out1Line << optflow_quality << "," << optflow_timeDel << "," << optflow_dist << ","; 121 | out1Line << imu_orientX << "," << imu_orientY << "," << imu_orientZ << "," << imu_orientW << ","; 122 | out1Line << imu_angularX << "," << imu_angularY << "," << imu_angularZ << ","; 123 | out1Line << imu_linAccelX << "," << imu_linAccelY << "," << imu_linAccelZ; 124 | out1Line.close(); 125 | 126 | gettimeofday(&stop,NULL); 127 | long int ms_stop = stop.tv_sec * 1000 + stop.tv_usec / 1000; 128 | outTimeHist << ms_stop-ms_start << "," << hdg << "," << rng << "," << temp << ","; 129 | outTimeHist << optflow_intTime << "," << optflow_intX << "," << optflow_intY << ","; 130 | outTimeHist << optflow_intXgyro << "," << optflow_intYgyro << "," << optflow_intZgyro << ","; 131 | outTimeHist << optflow_quality << "," << optflow_timeDel << "," << optflow_dist << ","; 132 | outTimeHist << imu_orientX << "," << imu_orientY << "," << imu_orientZ << "," << imu_orientW << ","; 133 | outTimeHist << imu_angularX << "," << imu_angularY << "," << imu_angularZ << ","; 134 | outTimeHist << imu_linAccelX << "," << imu_linAccelY << "," << imu_linAccelZ << std::endl; 135 | outTimeHist.close(); 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /src/pidWaypoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | mavros_msgs::State current_state; 16 | geometry_msgs::PoseStamped current_pose; 17 | 18 | const float POSITION_TOLERANCE = 0.1; 19 | const float ON_TARGET_THRESHOLD = 20; 20 | 21 | const float p = 0.2f; 22 | const float i = 0.0;//005f; 23 | const float d = 0.0f; 24 | 25 | 26 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 27 | { 28 | // ROS_INFO("I'm in state_cb!"); 29 | current_state = *msg; 30 | bool connected = current_state.connected; 31 | bool armed = current_state.armed; 32 | } 33 | 34 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr pose){ 35 | // ROS_INFO("I'm in pose_cb!"); 36 | current_pose = *pose; 37 | } 38 | 39 | float get_distance(double x1, double y1, double z1, double x2, double y2, double z2){ 40 | return sqrt(pow(x1-x2, 2) + pow(y1-y2, 2) + pow(z1-z2, 2)); 41 | } 42 | 43 | void go_to_position_pid(ros::NodeHandle* nh, float x, float y, float z){ 44 | ros::Publisher set_vel_pub = nh->advertise("mavros/setpoint_velocity/cmd_vel", 10); 45 | geometry_msgs::TwistStamped vel; 46 | 47 | 48 | if (set_vel_pub) 49 | { 50 | 51 | printf("Going to position: %f, %f, %f\n", x, y, z); 52 | 53 | int onTargetCount = 0; 54 | float accumulatedErrorX = 0; 55 | float accumulatedErrorY = 0; 56 | float accumulatedErrorZ = 0; 57 | 58 | while(true){ 59 | float distance = get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); 60 | 61 | printf("Current position: %f,%f,%f. Distance: %f\n", 62 | current_pose.pose.position.x, 63 | current_pose.pose.position.y, 64 | current_pose.pose.position.z, 65 | distance); 66 | 67 | float errorX = x - current_pose.pose.position.x; 68 | float errorY = y - current_pose.pose.position.y; 69 | float errorZ = z - current_pose.pose.position.z; 70 | 71 | accumulatedErrorX += errorX; 72 | accumulatedErrorY += errorY; 73 | accumulatedErrorZ += errorZ; 74 | 75 | float velX = errorX*p + accumulatedErrorX*i; 76 | float velY = errorY*p + accumulatedErrorY*i; 77 | float velZ = errorZ*p + accumulatedErrorZ*i; 78 | 79 | vel.twist.linear.x = velX; 80 | vel.twist.linear.y = velY; 81 | vel.twist.linear.z = velZ; 82 | 83 | // if(x > current_pose.pose.position.x){ 84 | // vel.twist.linear.x = velocity; 85 | // }else{ 86 | // vel.twist.linear.x = -velocity; 87 | // } 88 | // if(y > current_pose.pose.position.y){ 89 | // vel.twist.linear.y = velocity; 90 | // }else{ 91 | // vel.twist.linear.y = -velocity; 92 | // } 93 | // if(z > current_pose.pose.position.z){ 94 | // vel.twist.linear.z = velocity; 95 | // }else{ 96 | // vel.twist.linear.z = -velocity; 97 | // } 98 | 99 | set_vel_pub.publish(vel); 100 | sleep(0.5); 101 | set_vel_pub.publish(vel); 102 | 103 | if(distance < POSITION_TOLERANCE){ 104 | onTargetCount++; 105 | } 106 | 107 | if(onTargetCount > ON_TARGET_THRESHOLD){ 108 | break; 109 | } 110 | 111 | ros::spinOnce(); 112 | ros::Duration(0.1).sleep(); 113 | } 114 | 115 | vel.twist.linear.x = 0; 116 | vel.twist.linear.y = 0; 117 | vel.twist.linear.z = 0; 118 | set_vel_pub.publish(vel); 119 | sleep(0.5); 120 | set_vel_pub.publish(vel); 121 | 122 | ROS_INFO("Arrived at position"); 123 | } 124 | } 125 | 126 | 127 | 128 | int main(int argc, char** argv) 129 | { 130 | ros::init(argc, argv, "offb_node"); 131 | ros::NodeHandle nh; 132 | 133 | // the setpoint publishing rate MUST be faster than 2Hz 134 | ros::Rate rate(20.0); 135 | 136 | ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb); 137 | ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 10, pose_cb); 138 | // wait for FCU connection 139 | while (ros::ok() && !current_state.connected) 140 | { 141 | ros::spinOnce(); 142 | rate.sleep(); 143 | } 144 | 145 | ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 146 | 147 | //ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 148 | mavros_msgs::SetMode offb_set_mode; 149 | offb_set_mode.request.custom_mode = "GUIDED"; 150 | if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) 151 | ROS_INFO("OFFBOARD enabled"); 152 | else 153 | { 154 | ROS_INFO("unable to switch to offboard"); 155 | return -1; 156 | } 157 | 158 | sleep(2); 159 | 160 | // arming 161 | ros::ServiceClient arming_client_i = nh.serviceClient("mavros/cmd/arming"); 162 | mavros_msgs::CommandBool srv_arm_i; 163 | srv_arm_i.request.value = true; 164 | if (arming_client_i.call(srv_arm_i) && srv_arm_i.response.success) 165 | ROS_INFO("ARM sent %d", srv_arm_i.response.success); 166 | else 167 | { 168 | ROS_ERROR("Failed arming/disarming"); 169 | //return -1; 170 | } 171 | 172 | sleep(2); 173 | 174 | 175 | ros::ServiceClient takeoff_client = nh.serviceClient("/mavros/cmd/takeoff"); 176 | mavros_msgs::CommandTOL srv_takeoff; 177 | srv_takeoff.request.altitude = 2; 178 | // srv_takeoff.request.latitude = -35.3632607; 179 | // srv_takeoff.request.longitude = 149.1652351; 180 | srv_takeoff.request.min_pitch = 0; 181 | srv_takeoff.request.yaw = 0; 182 | if (takeoff_client.call(srv_takeoff) && srv_takeoff.response.success) 183 | ROS_INFO("takeoff sent %d", srv_takeoff.response.success); 184 | else 185 | { 186 | ROS_ERROR("Failed Takeoff"); 187 | return -1; 188 | } 189 | 190 | sleep(10); 191 | 192 | ROS_INFO("Going to position 1"); 193 | go_to_position_pid(&nh, 5,5,2); 194 | 195 | sleep(5); 196 | 197 | 198 | while (ros::ok()) 199 | { 200 | ros::spinOnce(); 201 | rate.sleep(); 202 | } 203 | return 0; 204 | } -------------------------------------------------------------------------------- /src/setDestination.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | geometry_msgs::PoseStamped waypoint; 8 | 9 | int main(int argc, char** argv) 10 | { 11 | ros::init(argc, argv, "setDestination"); 12 | 13 | ros::NodeHandle setDestinationNode; 14 | 15 | ros::Rate rate(5.0); 16 | ros::Publisher chatter_pub = setDestinationNode.advertise("TARwaypoint", 1); 17 | 18 | printf("desired x: "); 19 | fflush( stdout ); 20 | cin >> waypoint.pose.position.x; 21 | printf("desired y: "); 22 | fflush( stdout ); 23 | cin >> waypoint.pose.position.y; 24 | printf("desired z: "); 25 | fflush( stdout ); 26 | cin >> waypoint.pose.position.z; 27 | 28 | for (int i = 0; i < 20; ++i) 29 | { 30 | chatter_pub.publish(waypoint); 31 | ros::Duration(0.01).sleep(); 32 | } 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /src/setHeading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "std_msgs/Float64.h" 5 | 6 | using namespace std; 7 | 8 | std_msgs::Float64 newHeading; 9 | 10 | int main(int argc, char** argv) 11 | { 12 | ros::init(argc, argv, "setHeading"); 13 | 14 | ros::NodeHandle setHeadingNode; 15 | 16 | ros::Rate rate(5.0); 17 | ros::Publisher heading_pub = setHeadingNode.advertise("setHeading", 1); 18 | 19 | 20 | printf("desired heading: "); 21 | fflush( stdout ); 22 | cin >> newHeading.data; 23 | 24 | for (int i = 0; i < 20; ++i) 25 | { 26 | heading_pub.publish(newHeading); 27 | ros::Duration(0.01).sleep(); 28 | } 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /src/setHome.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | mavros_msgs::State current_state; 10 | 11 | uint8_t system_id = 255; 12 | uint8_t component_id = 1; 13 | uint8_t target_system = 1; 14 | 15 | bool packMavlinkMessage(const mavlink::Message& mavMsg, mavros_msgs::Mavlink &rosMsg) 16 | { 17 | mavlink::mavlink_message_t msg; 18 | mavlink::MsgMap map(msg); 19 | mavMsg.serialize(map); 20 | auto mi = mavMsg.get_message_info(); 21 | 22 | mavlink::mavlink_status_t *status = mavlink::mavlink_get_channel_status(mavlink::MAVLINK_COMM_0); 23 | status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; 24 | mavlink::mavlink_finalize_message_buffer(&msg, system_id, component_id, status, mi.min_length, mi.length, mi.crc_extra); 25 | 26 | return mavros_msgs::mavlink::convert(msg, rosMsg); 27 | } 28 | 29 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 30 | { 31 | current_state = *msg; 32 | } 33 | 34 | int main(int argc, char **argv) 35 | { 36 | ros::init(argc, argv, "set_home_node"); 37 | ros::NodeHandle home_handle; 38 | 39 | ros::Subscriber state_sub = home_handle.subscribe("mavros/state", 1, state_cb); 40 | ros::ServiceClient home_set = home_handle.serviceClient("/mavros/cmd/set_home",1); 41 | 42 | ros::Publisher origin_pub = home_handle.advertise("mavlink/to", 1000); 43 | 44 | ros::Rate rate(20.0); 45 | 46 | while(ros::ok() && current_state.connected) 47 | { 48 | ros::spinOnce(); 49 | rate.sleep(); 50 | } 51 | 52 | ros::Duration(2.0).sleep(); 53 | 54 | 55 | // double latitude = 30.2672; 56 | // double longitude = -97.7431; 57 | // double altitude = 165.0; 58 | double latitude = 0.0; 59 | double longitude = 0.0; 60 | double altitude = 0.0; 61 | 62 | 63 | mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN originMsg; 64 | originMsg.latitude = (uint32_t)(latitude * 10000000); 65 | originMsg.longitude = (uint32_t)(longitude * 10000000); 66 | originMsg.altitude = (uint32_t)(altitude * 1000); 67 | originMsg.target_system = target_system; 68 | 69 | mavros_msgs::Mavlink packedMsg; 70 | bool success = packMavlinkMessage(originMsg, packedMsg); 71 | 72 | if(success){ 73 | printf("Pack mavlink message SUCCEEDED\n"); 74 | } 75 | else{ 76 | printf("Pack mavlink message FAILED\n"); 77 | } 78 | 79 | origin_pub.publish(packedMsg); 80 | 81 | 82 | ros::Duration(2.0).sleep(); 83 | mavros_msgs::CommandHome set_home_req; 84 | set_home_req.request.current_gps = false; 85 | set_home_req.request.latitude = latitude; 86 | set_home_req.request.longitude = longitude; 87 | set_home_req.request.altitude = altitude; 88 | 89 | ros::service::call("/mavros/cmd/set_home", set_home_req); 90 | 91 | printf("Result was %d\n", set_home_req.response.result); 92 | 93 | } 94 | -------------------------------------------------------------------------------- /src/staple.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "std_msgs/Float64.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | //Set global variables 21 | mavros_msgs::State current_state; 22 | //nav_msgs::Odometry current_pose; 23 | geometry_msgs::PoseStamped current_pose; 24 | geometry_msgs::PoseStamped pose; 25 | std_msgs::Float64 current_heading; 26 | float GYM_OFFSET; 27 | 28 | //get armed state 29 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 30 | { 31 | current_state = *msg; 32 | bool connected = current_state.connected; 33 | bool armed = current_state.armed; 34 | } 35 | //get current position of drone 36 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) 37 | { 38 | current_pose = *msg; 39 | ROS_INFO("x: %f y: %f z: %f", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); 40 | // ROS_INFO("y: %f", current_pose.pose.position.y); 41 | // ROS_INFO("z: %f", current_pose.pose.position.z); 42 | } 43 | // void pose_cb(const nav_msgs::Odometry::ConstPtr& msg) 44 | // { 45 | // current_pose = *msg; 46 | // ROS_INFO("x: %f y: %f z: %f", current_pose.pose.pose.position.x, current_pose.pose.pose.position.y, current_pose.pose.pose.position.z); 47 | // } 48 | //get compass heading 49 | void heading_cb(const std_msgs::Float64::ConstPtr& msg) 50 | { 51 | current_heading = *msg; 52 | //ROS_INFO("current heading: %f", current_heading.data); 53 | } 54 | //set orientation of the drone (drone should always be level) 55 | void setHeading(float heading) 56 | { 57 | heading = -heading + 90 - GYM_OFFSET; 58 | float yaw = heading*(M_PI/180); 59 | float pitch = 0; 60 | float roll = 0; 61 | 62 | float cy = cos(yaw * 0.5); 63 | float sy = sin(yaw * 0.5); 64 | float cr = cos(roll * 0.5); 65 | float sr = sin(roll * 0.5); 66 | float cp = cos(pitch * 0.5); 67 | float sp = sin(pitch * 0.5); 68 | 69 | float qw = cy * cr * cp + sy * sr * sp; 70 | float qx = cy * sr * cp - sy * cr * sp; 71 | float qy = cy * cr * sp + sy * sr * cp; 72 | float qz = sy * cr * cp - cy * sr * sp; 73 | 74 | pose.pose.orientation.w = qw; 75 | pose.pose.orientation.x = qx; 76 | pose.pose.orientation.y = qy; 77 | pose.pose.orientation.z = qz; 78 | 79 | } 80 | // set position to fly to in the gym frame 81 | void setDestination(float x, float y, float z) 82 | { 83 | float deg2rad = (M_PI/180); 84 | float X = x*cos(-GYM_OFFSET*deg2rad) - y*sin(-GYM_OFFSET*deg2rad); 85 | float Y = x*sin(-GYM_OFFSET*deg2rad) + y*cos(-GYM_OFFSET*deg2rad); 86 | float Z = z; 87 | pose.pose.position.x = X; 88 | pose.pose.position.y = Y; 89 | pose.pose.position.z = Z; 90 | ROS_INFO("Destination set to x: %f y: %f z %f", X, Y, Z); 91 | } 92 | 93 | int main(int argc, char** argv) 94 | { 95 | ros::init(argc, argv, "offb_node"); 96 | ros::NodeHandle nh; 97 | 98 | // the setpoint publishing rate MUST be faster than 2Hz 99 | ros::Rate rate(20.0); 100 | ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb); 101 | ros::Publisher set_vel_pub = nh.advertise("mavros/setpoint_raw/local", 10); 102 | ros::Publisher local_pos_pub = nh.advertise("mavros/setpoint_position/local", 10); 103 | ros::Subscriber currentPos = nh.subscribe("/mavros/global_position/pose", 10, pose_cb); 104 | ros::Subscriber currentHeading = nh.subscribe("/mavros/global_position/compass_hdg", 10, heading_cb); 105 | 106 | // allow the subscribers to initialize 107 | ROS_INFO("INITILIZING..."); 108 | for(int i=0; i<100; i++) 109 | { 110 | ros::spinOnce(); 111 | ros::Duration(0.01).sleep(); 112 | } 113 | while(current_state.mode != "GUIDED") 114 | { 115 | ros::spinOnce(); 116 | ros::Duration(0.01).sleep(); 117 | } 118 | 119 | //set the orientation of the gym 120 | GYM_OFFSET = 0; 121 | for (int i = 1; i <= 30; ++i) { 122 | ros::spinOnce(); 123 | ros::Duration(0.1).sleep(); 124 | GYM_OFFSET += current_heading.data; 125 | ROS_INFO("current heading%d: %f", i, GYM_OFFSET/i); 126 | } 127 | GYM_OFFSET /= 30; 128 | ROS_INFO("the N' axis is facing: %f", GYM_OFFSET); 129 | cout << GYM_OFFSET << "\n" << endl; 130 | 131 | // wait for FCU connection 132 | while (ros::ok() && !current_state.connected) 133 | { 134 | ros::spinOnce(); 135 | rate.sleep(); 136 | } 137 | 138 | // arming 139 | ros::ServiceClient arming_client_i = nh.serviceClient("mavros/cmd/arming"); 140 | mavros_msgs::CommandBool srv_arm_i; 141 | srv_arm_i.request.value = true; 142 | if (arming_client_i.call(srv_arm_i) && srv_arm_i.response.success) 143 | ROS_INFO("ARM sent %d", srv_arm_i.response.success); 144 | else 145 | { 146 | ROS_ERROR("Failed arming"); 147 | return -1; 148 | } 149 | 150 | 151 | //request takeoff 152 | ros::ServiceClient takeoff_cl = nh.serviceClient("/mavros/cmd/takeoff"); 153 | mavros_msgs::CommandTOL srv_takeoff; 154 | srv_takeoff.request.altitude = 1.5; 155 | if(takeoff_cl.call(srv_takeoff)){ 156 | ROS_INFO("takeoff sent %d", srv_takeoff.response.success); 157 | }else{ 158 | ROS_ERROR("Failed Takeoff"); 159 | return -1; 160 | } 161 | 162 | sleep(10); 163 | 164 | 165 | //move foreward 166 | setHeading(0); 167 | setDestination(0, 2, 1.5); 168 | float tollorance = .35; 169 | if (local_pos_pub) 170 | { 171 | 172 | for (int i = 10000; ros::ok() && i > 0; --i) 173 | { 174 | 175 | local_pos_pub.publish(pose); 176 | // float percentErrorX = abs((pose.pose.position.x - current_pose.pose.position.x)/(pose.pose.position.x)); 177 | // float percentErrorY = abs((pose.pose.position.y - current_pose.pose.position.y)/(pose.pose.position.y)); 178 | // float percentErrorZ = abs((pose.pose.position.z - current_pose.pose.position.z)/(pose.pose.position.z)); 179 | // cout << " px " << percentErrorX << " py " << percentErrorY << " pz " << percentErrorZ << endl; 180 | // if(percentErrorX < tollorance && percentErrorY < tollorance && percentErrorZ < tollorance) 181 | // { 182 | // break; 183 | // } 184 | float deltaX = abs(pose.pose.position.x - current_pose.pose.position.x); 185 | float deltaY = abs(pose.pose.position.y - current_pose.pose.position.y); 186 | float deltaZ = abs(pose.pose.position.z - current_pose.pose.position.z); 187 | //cout << " dx " << deltaX << " dy " << deltaY << " dz " << deltaZ << endl; 188 | float dMag = sqrt( pow(deltaX, 2) + pow(deltaY, 2) + pow(deltaZ, 2) ); 189 | cout << dMag << endl; 190 | if( dMag < tollorance) 191 | { 192 | break; 193 | } 194 | ros::spinOnce(); 195 | ros::Duration(0.5).sleep(); 196 | if(i == 1) 197 | { 198 | ROS_INFO("Failed to reach destination. Stepping to next task."); 199 | } 200 | } 201 | ROS_INFO("Done moving foreward."); 202 | } 203 | 204 | //land 205 | ros::ServiceClient land_client = nh.serviceClient("/mavros/cmd/land"); 206 | mavros_msgs::CommandTOL srv_land; 207 | if (land_client.call(srv_land) && srv_land.response.success) 208 | ROS_INFO("land sent %d", srv_land.response.success); 209 | else 210 | { 211 | ROS_ERROR("Landing failed"); 212 | ros::shutdown(); 213 | return -1; 214 | } 215 | 216 | while (ros::ok()) 217 | { 218 | ros::spinOnce(); 219 | rate.sleep(); 220 | } 221 | return 0; 222 | } 223 | -------------------------------------------------------------------------------- /src/velTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavros_offboard_velocity_node.cpp 3 | * @brief MAVROS Offboard Control example node, written with mavros version 0.14.2, px4 flight 4 | * stack and tested in Gazebo SITL & jMAVSIM. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | mavros_msgs::State current_state; 21 | geometry_msgs::PoseStamped current_pose; 22 | 23 | const float POSITION_TOLERANCE = 1; 24 | 25 | const float p = 1.0f; 26 | const float i = 0.0f; 27 | const float d = 0.0f; 28 | 29 | void state_cb(const mavros_msgs::State::ConstPtr& msg) 30 | { 31 | // ROS_INFO("I'm in state_cb!"); 32 | current_state = *msg; 33 | bool connected = current_state.connected; 34 | bool armed = current_state.armed; 35 | } 36 | 37 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr pose){ 38 | // ROS_INFO("I'm in pose_cb!"); 39 | current_pose = *pose; 40 | } 41 | 42 | float get_distance(double x1, double y1, double z1, double x2, double y2, double z2){ 43 | return sqrt(pow(x1-x2, 2) + pow(y1-y2, 2) + pow(z1-z2, 2)); 44 | } 45 | 46 | bool go_to_position(ros::NodeHandle* nh, float x, float y, float z){ 47 | ros::Publisher set_pos_pub = nh->advertise("mavros/setpoint_position/local", 10); 48 | geometry_msgs::PoseStamped pos; 49 | 50 | pos.pose.position.x = x; 51 | pos.pose.position.y = y; 52 | pos.pose.position.z = z; 53 | 54 | if (set_pos_pub) 55 | { 56 | 57 | printf("Going to position: %f, %f, %f\n", pos.pose.position.x,pos.pose.position.y, pos.pose.position.z); 58 | 59 | set_pos_pub.publish(pos); 60 | ros::Duration(.5).sleep(); 61 | set_pos_pub.publish(pos); 62 | 63 | while(ros::ok() && get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z) > POSITION_TOLERANCE){ 64 | float distance = get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); 65 | 66 | printf("Current position: %f,%f,%f. Distance: %f\n", 67 | current_pose.pose.position.x, 68 | current_pose.pose.position.y, 69 | current_pose.pose.position.z, 70 | distance); 71 | ros::spinOnce(); 72 | ros::Duration(.25).sleep(); 73 | } 74 | 75 | ROS_INFO("Arrived at position"); 76 | } 77 | } 78 | 79 | bool go_to_position_raw(ros::NodeHandle* nh, float x, float y, float z, float velX, float velY, float velZ){ 80 | ros::Publisher set_pos_pub = nh->advertise("mavros/setpoint_raw/local", 10); 81 | mavros_msgs::PositionTarget pos; 82 | 83 | pos.position.x = x; 84 | pos.position.y = y; 85 | pos.position.z = z; 86 | 87 | pos.velocity.x = velX; 88 | pos.velocity.y = velY; 89 | pos.velocity.z = velZ; 90 | 91 | 92 | // pos.velocity 93 | 94 | if (set_pos_pub) 95 | { 96 | 97 | printf("Going to position: %f, %f, %f\n", pos.position.x,pos.position.y, pos.position.z); 98 | 99 | set_pos_pub.publish(pos); 100 | ros::Duration(.5).sleep(); 101 | set_pos_pub.publish(pos); 102 | 103 | while(ros::ok() && get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z) > POSITION_TOLERANCE){ 104 | float distance = get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); 105 | 106 | printf("Current position: %f,%f,%f. Distance: %f\n", 107 | current_pose.pose.position.x, 108 | current_pose.pose.position.y, 109 | current_pose.pose.position.z, 110 | distance); 111 | ros::spinOnce(); 112 | ros::Duration(.25).sleep(); 113 | } 114 | 115 | ROS_INFO("Arrived at position"); 116 | } 117 | } 118 | 119 | 120 | bool go_to_position_pid(ros::NodeHandle* nh, float x, float y, float z){ 121 | ros::Publisher set_vel_pub = nh->advertise("mavros/setpoint_velocity/cmd_vel", 10); 122 | geometry_msgs::TwistStamped vel; 123 | 124 | 125 | if (set_vel_pub) 126 | { 127 | 128 | printf("Going to position: %f, %f, %f\n", x, y, z); 129 | 130 | // set_vel_pub.publish(vel); 131 | // ros::Duration(.5).sleep(); 132 | // set_vel_pub.publish(vel); 133 | 134 | while(ros::ok() && get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z) > POSITION_TOLERANCE){ 135 | float distance = get_distance(x,y,z,current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); 136 | 137 | printf("Current position: %f,%f,%f. Distance: %f\n", 138 | current_pose.pose.position.x, 139 | current_pose.pose.position.y, 140 | current_pose.pose.position.z, 141 | distance); 142 | 143 | float errorX = x - current_pose.pose.position.x; 144 | float errorY = y - current_pose.pose.position.y; 145 | float errorZ = z - current_pose.pose.position.z; 146 | 147 | float velX = errorX * p; 148 | float velY = errorY * p; 149 | float velZ = errorZ * p; 150 | 151 | vel.twist.linear.x = velX; 152 | vel.twist.linear.y = velY; 153 | vel.twist.linear.z = velZ; 154 | 155 | // if(x > current_pose.pose.position.x){ 156 | // vel.twist.linear.x = velocity; 157 | // }else{ 158 | // vel.twist.linear.x = -velocity; 159 | // } 160 | // if(y > current_pose.pose.position.y){ 161 | // vel.twist.linear.y = velocity; 162 | // }else{ 163 | // vel.twist.linear.y = -velocity; 164 | // } 165 | // if(z > current_pose.pose.position.z){ 166 | // vel.twist.linear.z = velocity; 167 | // }else{ 168 | // vel.twist.linear.z = -velocity; 169 | // } 170 | 171 | set_vel_pub.publish(vel); 172 | sleep(0.5); 173 | set_vel_pub.publish(vel); 174 | 175 | ros::spinOnce(); 176 | ros::Duration(0.1).sleep(); 177 | } 178 | 179 | vel.twist.linear.x = 0; 180 | vel.twist.linear.y = 0; 181 | vel.twist.linear.z = 0; 182 | set_vel_pub.publish(vel); 183 | sleep(0.5); 184 | set_vel_pub.publish(vel); 185 | 186 | ROS_INFO("Arrived at position"); 187 | } 188 | } 189 | 190 | 191 | 192 | int main(int argc, char** argv) 193 | { 194 | ros::init(argc, argv, "offb_node"); 195 | ros::NodeHandle nh; 196 | 197 | // the setpoint publishing rate MUST be faster than 2Hz 198 | ros::Rate rate(20.0); 199 | 200 | ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb); 201 | ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 10, pose_cb); 202 | // wait for FCU connection 203 | while (ros::ok() && !current_state.connected) 204 | { 205 | ros::spinOnce(); 206 | rate.sleep(); 207 | } 208 | 209 | ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 210 | 211 | //ros::ServiceClient set_mode_client = nh.serviceClient("mavros/set_mode"); 212 | mavros_msgs::SetMode offb_set_mode; 213 | offb_set_mode.request.custom_mode = "GUIDED"; 214 | if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) 215 | ROS_INFO("OFFBOARD enabled"); 216 | else 217 | { 218 | ROS_INFO("unable to switch to offboard"); 219 | return -1; 220 | } 221 | 222 | sleep(2); 223 | 224 | // arming 225 | ros::ServiceClient arming_client_i = nh.serviceClient("mavros/cmd/arming"); 226 | mavros_msgs::CommandBool srv_arm_i; 227 | srv_arm_i.request.value = true; 228 | if (arming_client_i.call(srv_arm_i) && srv_arm_i.response.success) 229 | ROS_INFO("ARM sent %d", srv_arm_i.response.success); 230 | else 231 | { 232 | ROS_ERROR("Failed arming/disarming"); 233 | //return -1; 234 | } 235 | 236 | sleep(2); 237 | 238 | 239 | ros::ServiceClient takeoff_client = nh.serviceClient("/mavros/cmd/takeoff"); 240 | mavros_msgs::CommandTOL srv_takeoff; 241 | srv_takeoff.request.altitude = 2; 242 | // srv_takeoff.request.latitude = -35.3632607; 243 | // srv_takeoff.request.longitude = 149.1652351; 244 | srv_takeoff.request.min_pitch = 0; 245 | srv_takeoff.request.yaw = 0; 246 | if (takeoff_client.call(srv_takeoff) && srv_takeoff.response.success) 247 | ROS_INFO("takeoff sent %d", srv_takeoff.response.success); 248 | else 249 | { 250 | ROS_ERROR("Failed Takeoff"); 251 | return -1; 252 | } 253 | 254 | sleep(10); 255 | 256 | ROS_INFO("Going to position 1"); 257 | go_to_position_pid(&nh, 5,5,2); 258 | 259 | sleep(5); 260 | 261 | // ROS_INFO("Going to position 2"); 262 | // go_to_position(&nh, -5,5,3, 10); 263 | 264 | // sleep(5); 265 | 266 | // ROS_INFO("Going to position 3"); 267 | // go_to_position(&nh, -5,-5, 3, 10); 268 | 269 | // sleep(5); 270 | 271 | // ROS_INFO("Going to position 4"); 272 | // go_to_position(&nh, 5, -5, 3, 10); 273 | 274 | // sleep(5); 275 | 276 | // ROS_INFO("Going to position 5"); 277 | // go_to_position(&nh, 0, 0, 3, 10); 278 | 279 | 280 | while (ros::ok()) 281 | { 282 | ros::spinOnce(); 283 | rate.sleep(); 284 | } 285 | return 0; 286 | } --------------------------------------------------------------------------------