├── .github └── workflows │ └── manual.yml ├── CODEOWNERS ├── LICENSE ├── README.md └── quad_controller ├── CMakeLists.txt ├── INSTRUCTIONS.md ├── README.md ├── cfg ├── attitude_controller_params.cfg ├── hover_controller_params.cfg └── position_controller_params.cfg ├── config ├── attitude_controller_config.yaml └── position_controller_config.yaml ├── launch ├── attitude_controller.launch ├── hover_controller.launch └── position_controller.launch ├── msg └── EulerAngles.msg ├── output_data └── README.md ├── package.xml ├── scripts ├── attitude_controller_node ├── hover_controller_node ├── hover_twiddle_tuner_node ├── hover_zn_tuner_node ├── position_controller_node ├── quad_plotter_node ├── quaternion_to_euler_node ├── setup_2d_hover_world.sh └── setup_attitude_world.sh ├── setup.py ├── src └── quad_controller │ ├── __init__.py │ ├── pid_controller.py │ ├── plotting_helpers.py │ ├── twiddle.py │ └── utils │ └── test_run.py └── srv ├── GetPath.srv ├── SetFloat.srv ├── SetInt.srv ├── SetPath.srv └── SetPose.srv /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Udacity 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 | # Deprecated Repository 2 | This repository is deprecated. Currently enrolled learners, if any, can: 3 | - Utilize the https://knowledge.udacity.com/ forum to seek help on content-specific issues. 4 | - [Submit a support ticket](https://udacity.zendesk.com/hc/en-us/requests/new) if (learners are) blocked due to other reasons. 5 | 6 | 7 | [![Udacity - Robotics NanoDegree Program](https://s3-us-west-1.amazonaws.com/udacity-robotics/Extra+Images/RoboND_flag.png)](https://www.udacity.com/robotics) 8 | 9 | # Table of Contents # 10 | 11 | - [Hover Controller](#hover-controller) 12 | - [Attitude Controller](#attitude-controller) 13 | - [Position Controller](#position-controller) 14 | - [Additional Helpful Tools](#additional-helpful-tools) 15 | - [Using the Simulator](#using-the-simulator) 16 | 17 | # Hover Controller # 18 | ### Step 1: Writing the PID Class ### 19 | 20 | Add the PID class you wrote to `quad_controller/src/quad_controller/pid_controller.py` 21 | 22 | ### Step 2: Running the Hover Controller ### 23 | 24 | **NOTE: Launch `roscore` first, then Unity** 25 | 26 | After you have added the PID class, you can launch the hover controller. 27 | To do so, run the following commands in your terminal: 28 | ``` 29 | $ cd ~/catkin_ws/ 30 | $ catkin_make 31 | $ source devel/setup.bash 32 | $ roslaunch quad_controller hover_controller.launch 33 | ``` 34 | Now, with the hover controller launched, you can launch the quad simulator on your host platform. 35 | The details surrounding this process will be be different for each host platform (Win/Mac/Linux). 36 | please see "Using the Simulator" below. 37 | 38 | ### Step 3: Tuning Parameters ### 39 | Now that Unity has been launched, verify that you can see poses being published by the simulator 40 | on the `/quad_rotor/pose` topic: 41 | ``` 42 | $ rostopic echo /quad_rotor/pose 43 | ``` 44 | If you see messages being published, you're golden! If you don't see messages being published, 45 | then you might want to check your firewall settings on your host VM, or try restarting 46 | the simulator (at the moment there's an infrequent bug where comunications with the simulator does not work). 47 | 48 | Now that you've verified poses are being published, you can use `rqt_reconfigure` to adjust the setpoints 49 | and PID parameters for the hover controller! 50 | 51 | You should tune the parameters using rqt_reconfigure until you are happy with the result. 52 | For more information about how to tune PID parameters, head back into the classroom. 53 | 54 | If manually adjusting the PID parameters is not your thing, feel free to try out the 55 | Ziegler–Nichols tuning method using either `hover_zn_tuner_node`, or `hover_twiddle_tuner_node`. 56 | These nodes both implement tuning methods that were discussed in the classroom. 57 | 58 | You might find that these tuner helpers only get you part way to a good tune. 59 | There's a reason for this! Do you know? 60 | 61 | To run `hover_zn_tuner_node`: 62 | ``` 63 | $ rosrun quad_controller hover_zn_tuner_node 64 | ``` 65 | 66 | To run `twiddle_tuner_node`: 67 | ``` 68 | $ rosrun quad_controller hover_twiddle_tuner_node 69 | ``` 70 | 71 | Write down the parameters you came up with, as you will need them later, for the full positional controller! 72 | 73 | **Bonus Quesion:** 74 | 75 | Why does the quad overshoot more frequently when heading to a set point at a lower altitude? 76 | How can you modify the code to overcome this issue? 77 | 78 | # Attitude Controller # 79 | The attitude controller is responsible for controlling the roll, pitch, and yaw angle of the quad rotor. 80 | You can tune it very similarly to how you tuned the hover controller! 81 | Note: ZN/Twiddle Tuner nodes only work with the Hover controller. 82 | 83 | ### Step 1: Launch the Attitude Controller ### 84 | ``` 85 | $ roslaunch quad_controller attitude_controller.launch 86 | ``` 87 | 88 | ### Step 2: Launch the Attitude Controller ### 89 | 90 | Tune roll and pitch PID parmaeters until things look good. 91 | You'll also need to write down these PID parameters. 92 | As mentioned previously, you'll be using them in the positional controller! 93 | 94 | 95 | # Position Controller # 96 | 97 | Finally, now that you have tuned the attitude controller and positional controllers 98 | you can begin to work on the positional controller. The positional controller is 99 | responsible for commanding the attitude and thrust vectors in order to acheive a 100 | goal orientation in three dimensional space. 101 | 102 | Luckily, you've likely found some pretty decent PID parameters in your previous exploration. 103 | If you're fortunate, these parameters will work for you... However, even if they don't 104 | we've got you covered. There's all sorts of additional tooling that you can use to 105 | troubleshoot and tune positional control of your quad! 106 | 107 | ### Step 1: Launch the Positional Controller ### 108 | ``` 109 | $roslaunch quad_controller position_controller.launch 110 | ``` 111 | 112 | ### Step 2: Tuning Parameters ### 113 | 114 | Tune parameters until the controller is well-behaved. 115 | This should not be a very familiar, albeit potentially more difficult problem. 116 | 117 | # Additional Helpful Tools # 118 | With so many degrees of freedom, debugging and troubleshooting can be a painful process. 119 | In order to make things a little bit simpler, we've provided some tools that might make 120 | life a little bit easier. 121 | 122 | ### Constraining Forces and Torques ### 123 | 124 | It is possible to constrain forces and torques on the quad rotor's body frame. 125 | This can be useful if you're trying to debug only a single degree of freedom. 126 | 127 | Example: Disallow movement along the quad's X axis 128 | ``` 129 | $ rosservice call /quad_rotor/x_force_constrained "data: true" 130 | ``` 131 | Example: Disallow rotation about the quad's X axis 132 | ``` 133 | $ rosservice call /quad_rotor/x_torque_constrained "data: true" 134 | ``` 135 | 136 | ### Setting the Camera Pose ### 137 | 138 | To set the camera pose you can either, right click in the simulator, and drag 139 | or you can use the following service call, where the data parameter may take on the following 140 | values: 141 | 0: Orthogonal to the inertial frame's YZ plane, facing the positive X direction. 142 | 1: Orthogonal to the inertial frame's XZ plane, facing the positive Y direction. 143 | 2: Orthogonal to the intertial frame's XY plane, facing the negative Z direction. 144 | 3: Perspective view, facing the quad's body frame origin from the -X,-Y, +Z quadrant. 145 | 146 | ``` 147 | $ rosservice call /quad_rotor/camera_pose_type "data: 0" 148 | ``` 149 | To reset the camera pose, to the default pose, you can use the service call, or right click. 150 | 151 | ### Setting the Camera Distance ### 152 | 153 | To set the distance between the camera and the quad's body frame, you can use the 154 | `/quad_rotor/camera_distance` service. For example, to set the camera distance to be 155 | 20 meters, you would call the service as follows: 156 | ``` 157 | $ rosservice call /quad_rotor/camera_distance "data: 20.0" 158 | ``` 159 | 160 | To reset the camera distance to the default, simply right click in the simulator. 161 | 162 | ### Disabling Gravity ### 163 | 164 | Gravity can be a harsh reality. Particularly when you're dealing with attitude tuning. 165 | Fortunately, we can disable gravity in the simulator for the purposes of debugging. 166 | To do so, call the `/quad_rotor/gravity` service as follows: 167 | ``` 168 | $ rosservice call /quad_rotor/gravity "data: false" 169 | ``` 170 | 171 | ### Setting Pose ### 172 | 173 | To set the quad pose, use the `/quad_rotor/set_pose` service. The following service call 174 | will place the quad at the origin: 175 | ``` 176 | $ rosservice call /quad_rotor/set_pose "pose: 177 | position: 178 | x: 0.0 179 | y: 0.0 180 | z: 0.0 181 | orientation: 182 | x: 0.0 183 | y: 0.0 184 | z: 0.0 185 | w: 0.0" 186 | ``` 187 | 188 | ### Plotting using `quad_plotter_node` ### 189 | 190 | The `quad_plotter_node` is a handy tool which allows you to capture and plot quad movements. 191 | This might be useful to you while you are tuning the quad rotor in simulation. 192 | 193 | **Services:** 194 | - `/quad_plotter/start_recording` - Begins recording plot data (poses) 195 | - `/quad_plotter/stop_recording` - Stops recording plot data (poses) 196 | - `/quad_plotter/clear_path_history` - Clear plot history (poses) 197 | - `/quad_plotter/clear_waypoints` - Clear waypoints 198 | - `/quad_plotter/load_waypoints_from_sim` - Gets waypoints from the drone simulator 199 | - `/quad_plotter/get_path_history` - Returns path history as pose array 200 | - `/quad_plotter/plot_one` - Plots a 2D path on a given plane (*plane selection coming soon!*) 201 | - `/quad_plotter/plot_3d` - Create a 3D plot depicting path in perspective 202 | - `/quad_plotter/plot_grid` - Create a grid plot, showing 4 different views 203 | 204 | **Example: Capturing a Grid Plot** 205 | 1. Load waypoints from the simulator (optional) `$ rosservice call /quad_plotter/load_waypoints_from_sim` 206 | 2. Begin recording poses `$ rosservice call /quad_plotter/start_recording "{}"` 207 | 3. Perform the behavior that you wish to capture in the simulator. 208 | 4. Stop recording poses `$ rosservice call /quad_plotter/stop_recording "{}"` 209 | 5. Generate the plot `$ rosservice call /quad_plotter/plot_gird "{}"` 210 | 211 | After performing the above steps, a new timestamped PNG image should be generated and placed in the `/quad_controller/output_data` directory. 212 | 213 | # Using the Simulator # 214 | 215 | First be sure to grab the newest version of the simulator for your host computer OS [here](https://github.com/udacity/RoboND-Controls-Lab/releases). **Please note that you cannot use the simulator inside of the Udacity supplied course VM (Virtual Machine). You have to use the simulator for your host operating system and connect it to your VM.** 216 | 217 | If using the VM, inside the simulator's `_data` or `/Contents` folder, edit `ros_settings.txt` and set `vm-ip` to the VM’s IP address and set `vm-override` to `true`. If not using a VM, no edit is necessary. 218 | 219 | To find the ip of your VM, type `echo $(hostname -I)` into a terminal of your choice. **Be aware that the ip address of your VM can change. If you are experiencing problems, be sure to check that the VM's ip matches that of which you have in ros_settings.txt** 220 | 221 | **Note: Be sure to use at least V2.1.0 of the Udacity provided VM for this lab** 222 | -------------------------------------------------------------------------------- /quad_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(quad_controller) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 11 | dynamic_reconfigure 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | nav_msgs 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | EulerAngles.msg 57 | ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | SetFloat.srv 63 | SetInt.srv 64 | SetPose.srv 65 | SetPath.srv 66 | GetPath.srv 67 | ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | generate_messages( 78 | DEPENDENCIES 79 | std_msgs # Or other packages containing msgs 80 | geometry_msgs 81 | nav_msgs 82 | ) 83 | 84 | ################################################ 85 | ## Declare ROS dynamic reconfigure parameters ## 86 | ################################################ 87 | 88 | ## To declare and build dynamic reconfigure parameters within this 89 | ## package, follow these steps: 90 | ## * In the file package.xml: 91 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 92 | ## * In this file (CMakeLists.txt): 93 | ## * add "dynamic_reconfigure" to 94 | ## find_package(catkin REQUIRED COMPONENTS ...) 95 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 96 | ## and list every .cfg file to be processed 97 | 98 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 99 | generate_dynamic_reconfigure_options( 100 | cfg/attitude_controller_params.cfg 101 | cfg/position_controller_params.cfg 102 | cfg/hover_controller_params.cfg 103 | ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if you package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | # INCLUDE_DIRS include 116 | # LIBRARIES quad_controller 117 | # CATKIN_DEPENDS other_catkin_pkg 118 | # DEPENDS system_lib 119 | DEPENDS message_runtime 120 | ) 121 | 122 | ########### 123 | ## Build ## 124 | ########### 125 | 126 | ## Specify additional locations of header files 127 | ## Your package locations should be listed before other locations 128 | # include_directories(include) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/quad_controller.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/quad_controller_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_quad_controller.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /quad_controller/INSTRUCTIONS.md: -------------------------------------------------------------------------------- 1 | 2 | # Hover Controller # 3 | ** Step 1: ** 4 | Add the PID class you wrote to quad_controller/src/quad_controller/pid_controller.py 5 | 6 | ** Step 2: ** 7 | $catkin_make 8 | $ source deve/setup.bash 9 | $ roslaunch quad_controller hover_controller.launch 10 | 11 | ** Step 3: ** 12 | Launch Unity 13 | -Verify you can see from within vm (rostopic echo /quad_controller/pose 14 | Tune parameters in rqt_reconfigure until you are happy 15 | Use Twiddle and Zn scripts if you want, or use some other method. 16 | 17 | Bonus points: Why is it so hard to get a good tune when the quad is heading downwards? 18 | Can you fix this? (i,e, add mg term) 19 | 20 | Write down the parameters you came up with, you will need them later! 21 | 22 | # Attitude Controller # 23 | ** Step 1: ** 24 | $roslaunch quad_controller attitude_controller.launch 25 | 26 | ** Step 2: ** 27 | Tune roll and pitch PID parmaeters until things look good. 28 | Write down the parameters you came up with, once again, you will need them later. 29 | 30 | 31 | # Position Controller # 32 | ** Step 1: ** 33 | $roslaunch quad_controller position_controller.launch 34 | 35 | 36 | ** Step 2: ** 37 | Tune parameters until the controller is well-behaved 38 | 39 | ** Step 3: ** 40 | Run the Trajectory manager, feel free to use it to test your controller. 41 | 42 | ** Step 4: ** 43 | When you think it looks good, run the plotting tool, and share your score! 44 | -------------------------------------------------------------------------------- /quad_controller/README.md: -------------------------------------------------------------------------------- 1 | # Quad Controller Project # 2 | 3 | ### Launching the Simulator ### 4 | In order to launch the simulator, simply run the simulator binary. 5 | 6 | **Note:** 7 | You must first run `roscore`. If you don't run `roscore`, the 8 | simulator will still run, but you will not be able to communicate 9 | with it via ROS. 10 | 11 | ### The hover controller node ### 12 | Perhaps the simplest controller in the `quad_controller` package is the hover 13 | controller node. It is nothing more than a simple PID controller that sets 14 | applies a vertical thrust vector to the quad rotor body. 15 | 16 | #### Running the hover controller node #### 17 | 18 | ```sh 19 | $ rosrun quad_controller hover_controller_node 20 | 21 | ``` 22 | **Optional Step:** 23 | You can set up camera angles and motion constrants to make it 24 | make visualization and debugging of the hover conteroller 25 | simple. To do, simply run `setup_2d_hover_world.sh`. 26 | 27 | ```sh 28 | $ rosrun quad_controller setup_2d_hover_world.sh 29 | ``` 30 | 31 | **Hover controller parameter tuning:** 32 | The easiest way to tune the hover controller's PID parmaeters 33 | or set the target position is to use ROS' dynamic reconfigure. 34 | 35 | To run dynamic reconfigure: 36 | ```sh 37 | $ rqt_reconfigure 38 | ``` 39 | 40 | If `hover_controller_node` is running you should now be able 41 | to interactively explore the hover controller. 42 | 43 | **Plotting Altitude** 44 | One simple way to plot the altitude is to use `rqt_plot`. 45 | 46 | ```sh 47 | $ rqt_plot /quad_rotor/pose/pose/position/z 48 | ``` 49 | -------------------------------------------------------------------------------- /quad_controller/cfg/attitude_controller_params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | PACKAGE = "attitude_controller" 9 | 10 | from dynamic_reconfigure.parameter_generator_catkin import * 11 | import math 12 | 13 | gen = ParameterGenerator() 14 | 15 | gen.add('use_dr_set_point', bool_t, 0, 'Use dynamic reconfig setpoint', False) 16 | gen.add('roll_set_point', double_t, 0, 'Set Point: X', 0.0) 17 | gen.add('pitch_set_point', double_t, 0, 'Set Point: Y', 0.0) 18 | gen.add('yaw_set_point', double_t, 0, 'Set Point: Z', 0.0) 19 | 20 | gen.add('roll_kp', double_t, 0, 'Roll P', 0, 0, 100) 21 | gen.add('roll_ki', double_t, 0, 'Roll I', 0, 0, 100) 22 | gen.add('roll_ki_max', double_t, 0, 'Roll I Max', 0, 0, 100) 23 | gen.add('roll_kd', double_t, 0, 'Roll D', 0, 0, 100) 24 | 25 | gen.add('pitch_kp', double_t, 0, 'Pitch P', 0, 0, 100) 26 | gen.add('pitch_ki', double_t, 0, 'Pitch I', 0, 0, 100) 27 | gen.add('pitch_ki_max', double_t, 0, 'Pitch I Max', 0, 0, 100) 28 | gen.add('pitch_kd', double_t, 0, 'Pitch D', 0, 0, 100) 29 | 30 | gen.add('yaw_kp', double_t, 0, 'Yaw P', 0, 0, 100) 31 | gen.add('yaw_ki', double_t, 0, 'Yaw I', 0, 0, 100) 32 | gen.add('yaw_ki_max', double_t, 0, 'Yaw I Max', 0, 0, 100) 33 | gen.add('yaw_kd', double_t, 0, 'Yaw D', 0, 0, 100) 34 | 35 | exit(gen.generate(PACKAGE, 'attitude_controller', 'attitude_controller_params')) 36 | -------------------------------------------------------------------------------- /quad_controller/cfg/hover_controller_params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | PACKAGE = "hover_controller" 7 | 8 | from dynamic_reconfigure.parameter_generator_catkin import * 9 | 10 | gen = ParameterGenerator() 11 | 12 | gen.add('target', double_t, 0, 'Altitude Target', 0, 0, 100) 13 | gen.add('kp', double_t, 0, 'P', 0, 0, 100) 14 | gen.add('ki', double_t, 0, 'I', 0, 0, 100) 15 | gen.add('ki_max', double_t, 0, 'I Max Windup', 0, 0, 100 ) 16 | gen.add('kd', double_t, 0, 'D', 0, 0, 100) 17 | 18 | exit(gen.generate(PACKAGE, 'hover_controller', 'hover_controller_params')) 19 | -------------------------------------------------------------------------------- /quad_controller/cfg/position_controller_params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | PACKAGE = "position_controller" 7 | 8 | from dynamic_reconfigure.parameter_generator_catkin import * 9 | import math 10 | 11 | gen = ParameterGenerator() 12 | 13 | gen.add('use_dr_set_point', bool_t, 0, 'Use dynamic reconfig setpoint', False) 14 | gen.add('set_point_x', double_t, 0, 'Set Point: X', 0.0, -240.0, 240.0) 15 | gen.add('set_point_y', double_t, 0, 'Set Point: Y', 0.0, -240.0, 240.0) 16 | gen.add('set_point_z', double_t, 0, 'Set Point: Z', 0.0, 0.0, 100.0) 17 | 18 | gen.add('x_kp', double_t, 0, 'X P', 0, 0, 100) 19 | gen.add('x_ki', double_t, 0, 'X I', 0, 0, 100) 20 | gen.add('x_ki_max', double_t, 0, 'X Max Windup', 0, 0, 100) 21 | gen.add('x_kd', double_t, 0, 'X D', 0, 0, 100) 22 | 23 | gen.add('y_kp', double_t, 0, 'Y P', 0, 0, 100) 24 | gen.add('y_ki', double_t, 0, 'Y I', 0, 0, 100) 25 | gen.add('y_ki_max', double_t, 0, 'Y Max Windup', 0, 0, 100) 26 | gen.add('y_kd', double_t, 0, 'Y D', 0, 0, 100) 27 | 28 | gen.add('z_kp', double_t, 0, 'Z P', 0, 0, 100) 29 | gen.add('z_ki', double_t, 0, 'Z I', 0, 0, 100) 30 | gen.add('z_ki_max', double_t, 0, 'Z Max Windup', 0, 0, 100) 31 | gen.add('z_kd', double_t, 0, 'Z D', 0, 0, 100) 32 | 33 | exit(gen.generate(PACKAGE, 'position_controller', 'position_controller_params')) 34 | -------------------------------------------------------------------------------- /quad_controller/config/attitude_controller_config.yaml: -------------------------------------------------------------------------------- 1 | roll_kp: 0.0 2 | roll_ki: 0.0 3 | roll_ki_max: 0.0 4 | roll_kd: 0.0 5 | 6 | pitch_kp: 0.0 7 | pitch_ki: 0.0 8 | pitch_ki_max: 0.0 9 | pitch_kd: 0.0 10 | 11 | yaw_kp: 0.0 12 | yaw_ki: 0.0 13 | yaw_ki_max: 0.0 14 | yaw_kd: 0.0 15 | -------------------------------------------------------------------------------- /quad_controller/config/position_controller_config.yaml: -------------------------------------------------------------------------------- 1 | x_kp: 0.0 2 | x_ki: 0.0 3 | x_ki_max: 0.0 4 | x_kd: 0.0 5 | 6 | y_kp: 0.0 7 | y_ki: 0.0 8 | y_ki_max: 0.0 9 | y_kd: 0.0 10 | 11 | z_kp: 0.0 12 | z_ki: 0.0 13 | z_ki_max: 10 14 | z_kd: 0.0 15 | -------------------------------------------------------------------------------- /quad_controller/launch/attitude_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /quad_controller/launch/hover_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /quad_controller/launch/position_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /quad_controller/msg/EulerAngles.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 roll 3 | float64 pitch 4 | float64 yaw 5 | -------------------------------------------------------------------------------- /quad_controller/output_data/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/RoboND-Controls-Lab/5f9386cf5d5dff261a2a9b868f2be8f5639953b9/quad_controller/output_data/README.md -------------------------------------------------------------------------------- /quad_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | quad_controller 4 | 0.0.0 5 | The quad_controller package 6 | 7 | 8 | 9 | 10 | Brandon Kinman 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | message_generation 43 | dynamic_reconfigure 44 | dynamic_reconfigure 45 | message_runtime 46 | 47 | catkin 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /quad_controller/scripts/attitude_controller_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | import rospy 9 | import tf 10 | import math 11 | from sensor_msgs.msg import Imu 12 | from geometry_msgs.msg import Wrench 13 | from geometry_msgs.msg import Pose 14 | from geometry_msgs.msg import PoseStamped 15 | from geometry_msgs.msg import Vector3 16 | from std_msgs.msg import Float64 17 | 18 | from std_srvs.srv import Empty 19 | from std_srvs.srv import EmptyResponse 20 | 21 | from dynamic_reconfigure.server import Server 22 | from quad_controller.cfg import attitude_controller_paramsConfig 23 | from quad_controller.pid_controller import PIDController 24 | 25 | 26 | class AttitudeControllerNode(): 27 | def __init__(self): 28 | 29 | # Roll Controller 30 | roll_kp = float(rospy.get_param('~roll_kp', '0')) 31 | roll_ki = float(rospy.get_param('~roll_ki', '0')) 32 | roll_ki_max = float(rospy.get_param('~roll_ki_max', '0')) 33 | roll_kd = float(rospy.get_param('~roll_kd', '0')) 34 | self.roll_controller_ = PIDController(roll_kp, roll_ki, roll_kd, roll_ki_max) 35 | 36 | #Pitch Controller 37 | pitch_kp = float(rospy.get_param('~pitch_kp', '0')) 38 | pitch_ki = float(rospy.get_param('~pitch_ki', '0')) 39 | pitch_ki_max = float(rospy.get_param('~pitch_ki_max', '0')) 40 | pitch_kd = float(rospy.get_param('~pitch_kd', '0')) 41 | self.pitch_controller_ = PIDController(pitch_kp, pitch_ki, pitch_kd, pitch_ki_max) 42 | 43 | # Yaw controller 44 | yaw_kp = float(rospy.get_param('~yaw_kp', '0')) 45 | yaw_ki = float(rospy.get_param('~yaw_ki', '0')) 46 | yaw_ki_max = float(rospy.get_param('~yaw_ki_max', '0')) 47 | yaw_kd = float(rospy.get_param('~yaw_kd', '0')) 48 | self.yaw_controller_ = PIDController(yaw_kp, yaw_ki, yaw_kd, yaw_ki_max) 49 | 50 | self.last_imu_ = Imu() 51 | self.z_thrust_ = 0.0 52 | 53 | self.reset_srv_ = rospy.Service('attitude_controller/controller_reset', Empty, self.controller_reset_callback) 54 | 55 | srv = Server(attitude_controller_paramsConfig, self.config_callback) 56 | 57 | self.imu_sub = rospy.Subscriber("/quad_rotor/imu", Imu, self.imu_callback) 58 | self.cmd_force_pub = rospy.Publisher("/quad_rotor/cmd_force", Wrench, queue_size=10) 59 | self.cmd_attitude_sub = rospy.Subscriber('/quad_rotor/cmd_attitude', Vector3, self.cmd_attitude_callback) 60 | self.cmd_z_thrust_sub = rospy.Subscriber('/quad_rotor/cmd_z_thrust', Float64, self.cmd_z_thrust_callback) 61 | 62 | 63 | def config_callback(self, config, level): 64 | rospy.loginfo('Attitude controller config changed!') 65 | 66 | self.roll_controller_.setKP(config.roll_kp) 67 | self.roll_controller_.setKI(config.roll_ki) 68 | self.roll_controller_.setMaxWindup(config.roll_ki_max) 69 | self.roll_controller_.setKD(config.roll_kd) 70 | 71 | self.pitch_controller_.setKP(config.pitch_kp) 72 | self.pitch_controller_.setKI(config.pitch_ki) 73 | self.pitch_controller_.setMaxWindup(config.pitch_ki_max) 74 | self.pitch_controller_.setKD(config.pitch_kd) 75 | 76 | self.yaw_controller_.setKP(config.yaw_kp) 77 | self.yaw_controller_.setKI(config.yaw_ki) 78 | self.yaw_controller_.setMaxWindup(config.yaw_ki_max) 79 | self.yaw_controller_.setKD(config.yaw_kd) 80 | 81 | # Dynamic Reconfig Setpoint 82 | if config.use_dr_set_point: 83 | self.roll_controller_.setTarget(config.roll_set_point) 84 | self.pitch_controller_.setTarget(config.pitch_set_point) 85 | self.yaw_controller_.setTarget(config.yaw_set_point) 86 | 87 | return config 88 | 89 | def controller_reset_callback(self, pid_reset_msg): 90 | rospy.loginfo('Attitude controller reset!') 91 | self.roll_controller_.reset() 92 | self.pitch_controller_.reset() 93 | self.yaw_controller_.reset() 94 | 95 | self.last_imu_ = Imu() 96 | self.z_thrust_ = 0.0 97 | return EmptyResponse() 98 | 99 | def cmd_attitude_callback(self, attitude_vector): 100 | self.roll_controller_.setTarget(attitude_vector.x) 101 | self.pitch_controller_.setTarget(attitude_vector.y) 102 | self.yaw_controller_.setTarget(attitude_vector.z) 103 | 104 | def cmd_z_thrust_callback(self, cmd_z_thrust): 105 | self.z_thrust_ = cmd_z_thrust.data 106 | 107 | def imu_callback(self, imu_msg): 108 | self.last_imu_ = imu_msg 109 | self.update() 110 | 111 | def update(self): 112 | (roll_meas, pitch_meas, yaw_meas) = tf.transformations.euler_from_quaternion([self.last_imu_.orientation.x, 113 | self.last_imu_.orientation.y, 114 | self.last_imu_.orientation.z, 115 | self.last_imu_.orientation.w]) 116 | 117 | 118 | t = self.last_imu_.header.stamp.to_sec() 119 | roll_cmd = self.roll_controller_.update(roll_meas, t) 120 | pitch_cmd = self.pitch_controller_.update(pitch_meas, t) 121 | yaw_cmd = self.yaw_controller_.update(yaw_meas, t) 122 | 123 | 124 | rospy.logdebug('roll meas:{},pitch_meas:{},yaw_meas:{},x:{},y:{},z:{},w:{}'.format(roll_meas,pitch_meas,yaw_meas, 125 | self.last_imu_.orientation.x, 126 | self.last_imu_.orientation.y, 127 | self.last_imu_.orientation.z, 128 | self.last_imu_.orientation.w)) 129 | 130 | rospy.logdebug('In degrees: r:{},p:{},y:{}'.format(math.degrees(roll_meas), math.degrees(pitch_meas), math.degrees(yaw_meas))) 131 | 132 | wrench_cmd = Wrench() 133 | wrench_cmd.torque.x = roll_cmd 134 | wrench_cmd.torque.y = pitch_cmd 135 | wrench_cmd.torque.z = yaw_cmd 136 | wrench_cmd.force.z = self.z_thrust_ 137 | 138 | self.cmd_force_pub.publish(wrench_cmd) 139 | 140 | if __name__ == '__main__': 141 | rospy.init_node('attitude_controller') 142 | try: 143 | ac = AttitudeControllerNode() 144 | rospy.spin() 145 | except rospy.ROSInterruptException: 146 | pass 147 | -------------------------------------------------------------------------------- /quad_controller/scripts/hover_controller_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | import rospy 9 | import tf 10 | import math 11 | from geometry_msgs.msg import Wrench 12 | from geometry_msgs.msg import PoseStamped 13 | from quad_controller.pid_controller import PIDController 14 | 15 | from dynamic_reconfigure.server import Server 16 | from quad_controller.cfg import hover_controller_paramsConfig 17 | 18 | class HoverControllerNode(): 19 | def __init__(self): 20 | self.prev_time = rospy.Time.now() 21 | 22 | # PID Params 23 | ki_max = float(rospy.get_param('~ki_max', '20')) 24 | kp = float(rospy.get_param('kp', '0')) 25 | ki = float(rospy.get_param('ki', '0')) 26 | kd = float(rospy.get_param('kd', '0')) 27 | 28 | self.controller_ = PIDController(kp, ki, kd, ki_max) 29 | 30 | self.pose_sub_ = rospy.Subscriber("/quad_rotor/pose", PoseStamped, self.pose_callback) 31 | self.cmd_force_pub_ = rospy.Publisher("/quad_rotor/cmd_force", Wrench, queue_size=10) 32 | 33 | srv = Server(hover_controller_paramsConfig, self.config_callback) 34 | 35 | def config_callback(self, config, level): 36 | rospy.loginfo("""Reconfigure Request: {target},\ 37 | {kp}, {ki}, {kd}""".format(**config)) 38 | 39 | self.controller_.setTarget(config.target) 40 | self.controller_.setKP(config.kp) 41 | self.controller_.setKI(config.ki) 42 | self.controller_.setMaxWindup(config.ki_max) 43 | self.controller_.setKD(config.kd) 44 | return config 45 | 46 | def pose_callback(self, pose_msg): 47 | z_cmd = self.controller_.update(pose_msg.pose.position.z, pose_msg.header.stamp.to_sec()) 48 | cmd = Wrench() 49 | 50 | if z_cmd is not None: 51 | cmd.force.z = z_cmd 52 | self.cmd_force_pub_.publish(cmd) 53 | 54 | if __name__ == '__main__': 55 | rospy.init_node('hover_controller') 56 | try: 57 | hc = HoverControllerNode() 58 | rospy.spin() 59 | except rospy.ROSInterruptException: 60 | pass 61 | -------------------------------------------------------------------------------- /quad_controller/scripts/hover_twiddle_tuner_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | import rospy 9 | import dynamic_reconfigure.client 10 | from std_srvs.srv import Empty 11 | from std_srvs.srv import SetBool 12 | from geometry_msgs.msg import PoseStamped 13 | from geometry_msgs.msg import Pose 14 | from geometry_msgs.msg import Wrench 15 | from quad_controller.twiddle import Twiddle 16 | from quad_controller.srv import SetPose 17 | from quad_controller.srv import SetInt 18 | 19 | 20 | class HoverTestRun: 21 | def __init__(self): 22 | self.is_started_ = False 23 | self.test_data_ = [] 24 | self.oscillation_count_ = 0 25 | self.max_duration_ = 10 26 | self.start_time_ = 0 27 | self.duration_ = 0 28 | 29 | def addPose(self ,pose_stamped): 30 | if self.is_started_ is False: 31 | return 32 | if self.start_time_ is 0: 33 | self.start_time_ = pose_stamped.header.stamp 34 | return 35 | self.test_data_.append((pose_stamped.header.stamp, pose_stamped.pose.position.z)) 36 | 37 | def computeTotalError(self, set_point): 38 | total_error = 0 39 | for item in self.test_data_: 40 | total_error += abs(set_point - item[1]) 41 | return total_error 42 | 43 | 44 | def isFinished(self): 45 | if self.is_started_ is False or len(self.test_data_) is 0: 46 | return False 47 | 48 | # Test is finished if we've reached max duration 49 | duration = self.test_data_[-1][0] - self.start_time_ 50 | if duration.to_sec() > self.max_duration_: 51 | self.is_started_ = False 52 | return True 53 | else: 54 | return False 55 | 56 | # Test if finished it the number of oscillations are 57 | 58 | def reset(self): 59 | self.__init__() 60 | 61 | def start(self): 62 | self.is_started_ = True 63 | 64 | 65 | def initial_setup(): 66 | rospy.wait_for_service('/quad_rotor/reset_orientation') 67 | rospy.wait_for_service('/quad_rotor/set_pose') 68 | rospy.wait_for_service('/quad_rotor/x_force_constrained') 69 | rospy.wait_for_service('/quad_rotor/y_force_constrained') 70 | rospy.wait_for_service('/quad_rotor/x_torque_constrained') 71 | rospy.wait_for_service('/quad_rotor/y_torque_constrained') 72 | rospy.wait_for_service('/quad_rotor/z_torque_constrained') 73 | rospy.wait_for_service('/quad_rotor/camera_pose_type') 74 | 75 | try: 76 | # Reset the forces and velocities on the quad 77 | reset_force_vel = rospy.ServiceProxy('/quad_rotor/reset_orientation', SetBool) 78 | reset_force_vel(True) 79 | 80 | # Call service to set position 81 | set_position = rospy.ServiceProxy('/quad_rotor/set_pose', SetPose) 82 | initial_pose = Pose() 83 | initial_pose.position.z = 10 84 | response = set_position(initial_pose) 85 | 86 | # Call service to constrain translations. We only want to be able to translate in Z 87 | x_force_constrained = rospy.ServiceProxy('/quad_rotor/x_force_constrained', SetBool) 88 | y_force_constrained = rospy.ServiceProxy('/quad_rotor/y_force_constrained', SetBool) 89 | x_force_constrained(True) 90 | y_force_constrained(True) 91 | 92 | # Call service to constrian rotations, we don't want to rotate at All 93 | x_torque_constrained = rospy.ServiceProxy('/quad_rotor/x_torque_constrained', SetBool) 94 | y_torque_constrained = rospy.ServiceProxy('/quad_rotor/y_torque_constrained', SetBool) 95 | z_torque_constrained = rospy.ServiceProxy('/quad_rotor/z_torque_constrained', SetBool) 96 | x_torque_constrained(True) 97 | y_torque_constrained(True) 98 | z_torque_constrained(True) 99 | 100 | #Set camera pose to be aligned to X-axis (1) 101 | camera_pose_type = rospy.ServiceProxy('/quad_rotor/camera_pose_type', SetInt) 102 | camera_pose_type(1) 103 | except rospy.ServiceException, e: 104 | rospy.logerr('Service call failed: {}'.format(e)) 105 | 106 | def setup_test(): 107 | # Reset the forces and velocities on the quad 108 | reset_force_vel = rospy.ServiceProxy('/quad_rotor/reset_orientation', SetBool) 109 | reset_force_vel(True) 110 | 111 | # Call service to set position 112 | set_position = rospy.ServiceProxy('/quad_rotor/set_pose', SetPose) 113 | initial_pose = Pose() 114 | initial_pose.position.z = 10 115 | response = set_position(initial_pose) 116 | 117 | def algorithm(pid_params): 118 | setup_test() 119 | # Make the movement happen, stopping when some criteria has been met 120 | dyn_reconf_client.update_configuration({'target': 20.0, 121 | 'kp': pid_params[0], 122 | 'ki': pid_params[1], 123 | 'kd': pid_params[2]}) 124 | curr_test_run.reset() 125 | curr_test_run.start() 126 | 127 | while not curr_test_run.isFinished(): 128 | rate.sleep() 129 | 130 | error = curr_test_run.computeTotalError(20.0) 131 | 132 | # return the error, here lower error is better. 133 | return error 134 | 135 | def pose_callback(pose): 136 | curr_test_run.addPose(pose) 137 | 138 | def dyn_reconf_callback(config): 139 | # rospy.loginfo("""Target:{target}, kp:{kp}, ki:{ki}, kd:{kd}""".format(**config)) 140 | pass 141 | 142 | def main(): 143 | initial_params = [0.0, 0.0, 0.0] 144 | twiddle_hover = Twiddle(algorithm, initial_params) 145 | 146 | print("Initial Params: kp:{}, ki:{}, kd:{}".format(initial_params[0], initial_params[1], initial_params[2])) 147 | 148 | #while forever, just run the twiddle and print out the results 149 | while not rospy.is_shutdown(): 150 | (iterations, params, best_run) = twiddle_hover.run() 151 | print('iterations: {} params: {} best_run: {}'.format(iterations, params, best_run)) 152 | 153 | if __name__ == '__main__': 154 | curr_test_run = HoverTestRun() 155 | curr_test_run.start() 156 | rospy.init_node('hover_twiddle_tuner') 157 | rate = rospy.Rate(10) 158 | 159 | pose_sub_ = rospy.Subscriber("/quad_rotor/pose", PoseStamped, pose_callback) 160 | cmd_force_pub_ = rospy.Publisher("/quad_rotor/cmd_force", Wrench, queue_size=10) 161 | dyn_reconf_client = dynamic_reconfigure.client.Client('/hover_controller', config_callback=dyn_reconf_callback) 162 | 163 | initial_setup() 164 | 165 | try: 166 | main() 167 | except rospy.ROSInterruptException: 168 | pass 169 | -------------------------------------------------------------------------------- /quad_controller/scripts/hover_zn_tuner_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import dynamic_reconfigure.client 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | import rospy 6 | 7 | from geometry_msgs.msg import Pose 8 | from geometry_msgs.msg import PoseStamped 9 | from quad_controller.srv import SetPose 10 | from quad_controller.srv import SetInt 11 | from std_srvs.srv import SetBool 12 | 13 | def plotRun(samples, title, axis_labels): 14 | t_init = samples[0][1].to_sec() 15 | sample_times = [ sample[1].to_sec() - t_init for sample in samples ] 16 | sample_values = [ sample[0] for sample in samples ] 17 | 18 | fig = plt.figure(figsize=(12,3)) 19 | plt.subplot(111) 20 | plt.title(title) 21 | plt.xlabel(axis_labels[0]) 22 | plt.ylabel(axis_labels[1]) 23 | plt.plot(sample_times, sample_values) 24 | plt.plot((sample_times[0], sample_times[-1]), 25 | (10, 10), 'k--') 26 | plt.show() 27 | 28 | def reset_pose_and_dynamics(): 29 | try: 30 | # Reset the forces and velocities on the quad 31 | reset_force_vel = rospy.ServiceProxy('/quad_rotor/reset_orientation', SetBool) 32 | reset_force_vel(True) 33 | 34 | # Call service to set position 35 | set_position = rospy.ServiceProxy('/quad_rotor/set_pose', SetPose) 36 | initial_pose = Pose() 37 | initial_pose.position.z = 0 38 | response = set_position(initial_pose) 39 | except rospy.ServiceException, e: 40 | rospy.logerr('Service call failed: {}'.format(e)) 41 | 42 | 43 | def set_kp(kp): 44 | dr_client.update_configuration({'kp': kp}) 45 | 46 | def set_target(target): 47 | dr_client.update_configuration({'target': target}) 48 | 49 | def dr_callback(config): 50 | kp = config.kp 51 | ki = config.ki 52 | kd = config.kd 53 | target = config.target 54 | rospy.loginfo('config set: target:{}, kp:{}, ki:{}, kd:{}'.format(target, kp, ki, kd)) 55 | 56 | def pose_callback(pose): 57 | global current_altitude 58 | global current_time 59 | global samples 60 | current_altitude = pose.pose.position.z 61 | current_time = pose.header.stamp 62 | samples.append([pose.pose.position.z, pose.header.stamp]) 63 | 64 | 65 | def setup_experiment(): 66 | rospy.wait_for_service('/quad_rotor/reset_orientation') 67 | rospy.wait_for_service('/quad_rotor/set_pose') 68 | rospy.wait_for_service('/quad_rotor/x_force_constrained') 69 | rospy.wait_for_service('/quad_rotor/y_force_constrained') 70 | rospy.wait_for_service('/quad_rotor/x_torque_constrained') 71 | rospy.wait_for_service('/quad_rotor/y_torque_constrained') 72 | rospy.wait_for_service('/quad_rotor/z_torque_constrained') 73 | rospy.wait_for_service('/quad_rotor/camera_pose_type') 74 | 75 | try: 76 | # Reset the forces and velocities on the quad 77 | reset_force_vel = rospy.ServiceProxy('/quad_rotor/reset_orientation', SetBool) 78 | reset_force_vel(True) 79 | 80 | # Call service to set position 81 | set_position = rospy.ServiceProxy('/quad_rotor/set_pose', SetPose) 82 | initial_pose = Pose() 83 | initial_pose.position.z = 0 84 | response = set_position(initial_pose) 85 | 86 | # Call service to constrain translations. We only want to be able to translate in Z 87 | x_force_constrained = rospy.ServiceProxy('/quad_rotor/x_force_constrained', SetBool) 88 | y_force_constrained = rospy.ServiceProxy('/quad_rotor/y_force_constrained', SetBool) 89 | x_force_constrained(True) 90 | y_force_constrained(True) 91 | 92 | # Call service to constrian rotations, we don't want to rotate at All 93 | x_torque_constrained = rospy.ServiceProxy('/quad_rotor/x_torque_constrained', SetBool) 94 | y_torque_constrained = rospy.ServiceProxy('/quad_rotor/y_torque_constrained', SetBool) 95 | z_torque_constrained = rospy.ServiceProxy('/quad_rotor/z_torque_constrained', SetBool) 96 | x_torque_constrained(True) 97 | y_torque_constrained(True) 98 | z_torque_constrained(True) 99 | 100 | #Set camera pose to be aligned to X-axis (1) 101 | camera_pose_type = rospy.ServiceProxy('/quad_rotor/camera_pose_type', SetInt) 102 | camera_pose_type(1) 103 | except rospy.ServiceException, e: 104 | rospy.logerr('Service call failed: {}'.format(e)) 105 | 106 | def zn_tune(max_duration, kmin, kmax, kstep, rule='classic'): 107 | global samples 108 | tune_rate_hz = 10.0 109 | tune_rate = rospy.Rate(tune_rate_hz) 110 | 111 | for kp in np.linspace(kmin, kmax, (kmax-kmin)/kstep): 112 | set_kp(kp) 113 | reset_pose_and_dynamics() 114 | 115 | #Clear samples for each test run 116 | samples = [] 117 | 118 | oscillation_count = 0 119 | oscillation_times = [] 120 | initial_error = 0 121 | 122 | for i in range(0, int(max_duration*tune_rate_hz)): 123 | tune_rate.sleep() 124 | if i is 0: 125 | initial_error = target - current_altitude 126 | new_error = target - current_altitude 127 | 128 | #Check for oscillsation 129 | if (cmp(initial_error, 0) != cmp(new_error, 0)) & (cmp(new_error, 0) != 0) & (cmp(initial_error, 0) != 0): 130 | rospy.loginfo('Oscillation Detected! ({})'.format(oscillation_count)) 131 | oscillation_count += 1 132 | oscillation_times.append(current_time) 133 | initial_error = new_error 134 | 135 | # One period is 3 oscillations 136 | if oscillation_count is 3: 137 | # Calculate tu 138 | tu = oscillation_times[2] - oscillation_times[0] 139 | rospy.loginfo('Oscillation Period (Tu): {}'.format(tu.to_sec())) 140 | 141 | # Check to make sure oscillation is significant 142 | if tu.to_sec() > 3.0 * 1.0/tune_rate_hz: 143 | 144 | if rule == 'classic': 145 | kp_zn = 0.6*kp 146 | ki_zn = 1.2*kp/tu.to_sec() 147 | kd_zn = 0.075*kp*tu.to_sec() 148 | 149 | if rule == 'no-overshoot': 150 | kp_zn = 0.2*kp 151 | ki_zn = 0.4*kp/tu.to_sec() 152 | kd_zn = 0.2*kp*tu.to_sec()/3.0 153 | 154 | if rule == 'some-overshoot': 155 | kp_zn = 0.33*kp 156 | ki_zn = 0.66*kp/tu.to_sec() 157 | kd_zn = 0.1089*kp*tu.to_sec() 158 | 159 | rospy.loginfo('Computed Params! kp:{}, ki:{}, kd:{}'.format(kp_zn, ki_zn, kd_zn)) 160 | return (kp_zn, ki_zn, kd_zn) 161 | else: 162 | #Try a larger Kp 163 | break 164 | rospy.loginfo('Found no oscillations in search range range ({}-{})'.format(kmin,kmax)) 165 | return (0, 0, 0) 166 | 167 | if __name__ == '__main__': 168 | kp = 0 169 | ki = 0 170 | kd = 0 171 | target = 10 172 | current_altitude = 0 173 | current_time = 0 174 | samples = [] 175 | 176 | rospy.init_node('hover_zn_tuner') 177 | dr_client = dynamic_reconfigure.client.Client('/hover_controller', config_callback=dr_callback) 178 | pose_sub_ = rospy.Subscriber("/quad_rotor/pose", PoseStamped, pose_callback) 179 | 180 | setup_experiment() 181 | set_target(target) 182 | (kp_zn, ki_zn, kd_zn) = zn_tune(10.0, 5, 10 , 1, rule='no-overshoot') 183 | dr_client.update_configuration({'kp': kp_zn, 'ki': ki_zn, 'kd':kd_zn}) 184 | plotRun(samples,'ZN Tuning Run: no-overshoot', ['Time','Altitude']) 185 | 186 | reset_pose_and_dynamics() 187 | samples = [] 188 | set_target(10) 189 | rospy.sleep(10) 190 | plotRun(samples,'ZN Test Run: no-overshoot ',['Time','Altitude']) 191 | 192 | rospy.signal_shutdown('finished') -------------------------------------------------------------------------------- /quad_controller/scripts/position_controller_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | import rospy 9 | import tf 10 | import math 11 | import numpy as np 12 | from geometry_msgs.msg import Wrench 13 | from geometry_msgs.msg import PoseStamped 14 | from geometry_msgs.msg import Point 15 | from geometry_msgs.msg import Vector3 16 | from std_msgs.msg import Float64 17 | from std_msgs.msg import Bool 18 | from quad_controller.pid_controller import PIDController 19 | 20 | from std_srvs.srv import SetBool 21 | from std_srvs.srv import SetBoolResponse 22 | 23 | from std_srvs.srv import Empty 24 | from std_srvs.srv import EmptyResponse 25 | 26 | from dynamic_reconfigure.server import Server 27 | from quad_controller.cfg import position_controller_paramsConfig 28 | 29 | def limit(value, min_value, max_value): 30 | if value > max_value: 31 | value = max_value 32 | elif value < min_value: 33 | value = min_value 34 | return value 35 | 36 | class PositionControllerNode(): 37 | def __init__(self): 38 | self.first_pose_received_ = False 39 | self.counter = 0 40 | 41 | # X PID Config 42 | x_kp = float(rospy.get_param('x_kp', '0')) 43 | x_ki = float(rospy.get_param('x_ki', '0')) 44 | x_ki_max = float(rospy.get_param('x_ki_max', '0')) 45 | x_kd = float(rospy.get_param('x_kd', '0')) 46 | self.x_controller_ = PIDController(x_kp, x_ki, x_kd, x_ki_max) 47 | 48 | # Y PID Config 49 | y_kp = float(rospy.get_param('y_kp', '0')) 50 | y_ki = float(rospy.get_param('y_ki', '0')) 51 | y_ki_max = float(rospy.get_param('y_ki_max', '0')) 52 | y_kd = float(rospy.get_param('y_kd', '0')) 53 | self.y_controller_ = PIDController(y_kp, y_ki, y_kd, y_ki_max) 54 | 55 | # Z PID Config 56 | z_kp = float(rospy.get_param('z_kp', '0')) 57 | z_ki = float(rospy.get_param('z_ki', '0')) 58 | z_ki_max = float(rospy.get_param('z_ki_max', '0')) 59 | z_kd = float(rospy.get_param('z_kd', '0')) 60 | self.z_controller_ = PIDController(z_kp, z_ki, z_kd, z_ki_max) 61 | 62 | self.pose_sub_ = rospy.Subscriber('/quad_rotor/pose', PoseStamped, self.pose_callback) 63 | 64 | self.goal_point_sub_ = rospy.Subscriber('/quad_rotor/goal_point', Point, self.goal_point_callback) 65 | self.reset_srv_ = rospy.Service('/position_controller/controller_reset', Empty, self.controller_reset_callback) 66 | 67 | self.cmd_z_thrust_pub_ = rospy.Publisher('/quad_rotor/cmd_z_thrust', Float64, queue_size=10) 68 | self.cmd_attitude_pub_ = rospy.Publisher('/quad_rotor/cmd_attitude', Vector3, queue_size=10) 69 | 70 | srv = Server(position_controller_paramsConfig, self.config_callback) 71 | 72 | def config_callback(self, config, level): 73 | rospy.loginfo('Position controller node config changed!') 74 | 75 | # X Config Params 76 | self.x_controller_.setKP(config.x_kp) 77 | self.x_controller_.setKI(config.x_ki) 78 | self.x_controller_.setMaxWindup(config.x_ki_max) 79 | self.x_controller_.setKD(config.x_kd) 80 | 81 | # Y Config Params 82 | self.y_controller_.setKP(config.y_kp) 83 | self.y_controller_.setKI(config.y_ki) 84 | self.y_controller_.setMaxWindup(config.y_ki_max) 85 | self.y_controller_.setKD(config.y_kd) 86 | 87 | # Z Config Params 88 | self.z_controller_.setKP(config.z_kp) 89 | self.z_controller_.setKI(config.z_ki) 90 | self.z_controller_.setMaxWindup(config.z_ki_max) 91 | self.z_controller_.setKD(config.z_kd) 92 | 93 | # Dynamic Reconfig Setpoint 94 | if config.use_dr_set_point: 95 | self.x_controller_.setTarget(config.set_point_x) 96 | self.y_controller_.setTarget(config.set_point_y) 97 | self.z_controller_.setTarget(config.set_point_z) 98 | 99 | return config 100 | 101 | def controller_reset_callback(self, controller_reset_msg): 102 | rospy.loginfo('Position controller reset!') 103 | self.x_controller_.reset() 104 | self.y_controller_.reset() 105 | self.z_controller_.reset() 106 | self.first_pose_received = False 107 | return EmptyResponse() 108 | 109 | def goal_point_callback(self, goal_point_msg): 110 | self.x_controller_.setTarget(goal_point_msg.x) 111 | self.y_controller_.setTarget(goal_point_msg.y) 112 | self.z_controller_.setTarget(goal_point_msg.z) 113 | 114 | def pose_callback(self, pose_msg): 115 | 116 | if self.counter >= 10 or not self.first_pose_received_: 117 | self.counter = 0 118 | 119 | # Initial goal should be starting position. 120 | if self.first_pose_received_ is False: 121 | self.first_pose_received_ = True 122 | self.goal_point_callback(pose_msg.pose.position) 123 | 124 | t = pose_msg.header.stamp.to_sec() 125 | 126 | # Control Roll to to move along Y 127 | roll_cmd = -(1/9.81) * self.y_controller_.update(pose_msg.pose.position.y, t) 128 | 129 | # Control Pitch to move along X 130 | pitch_cmd = (1/9.81) * self.x_controller_.update(pose_msg.pose.position.x, t) 131 | 132 | # Control Thrust to move along Z 133 | thrust = self.z_controller_.update(pose_msg.pose.position.z, t) 134 | 135 | roll = limit(roll_cmd, -15*(math.pi/180), +15*(math.pi/180)) 136 | pitch = limit(pitch_cmd, -15*(math.pi/180), +15*(math.pi/180)) 137 | 138 | rpy_cmd = Vector3() 139 | cmd_z_thrust = Float64() 140 | 141 | rpy_cmd.x = roll 142 | rpy_cmd.y = pitch 143 | 144 | # Compensate for g and clip thrust 145 | cmd_z_thrust.data = limit(thrust + 2 * 9.81, -30+2*9.81, 30+2*9.81) 146 | 147 | rospy.logdebug('roll:{}, pitch:{}, z_thrust:{}'.format(roll, pitch, thrust)) 148 | self.cmd_attitude_pub_.publish(rpy_cmd) 149 | self.cmd_z_thrust_pub_.publish(cmd_z_thrust) 150 | else: 151 | self.counter += 1 152 | 153 | if __name__ == '__main__': 154 | rospy.init_node('position_controller') 155 | try: 156 | ac = PositionControllerNode() 157 | rospy.spin() 158 | except rospy.ROSInterruptException: 159 | pass 160 | -------------------------------------------------------------------------------- /quad_controller/scripts/quad_plotter_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: Devin Anzelmo 4 | 5 | # TODO add services for setting some plot parameters to 6 | from nav_msgs.srv import GetPlan 7 | from nav_msgs.srv import GetPlanRequest 8 | from nav_msgs.srv import GetPlanResponse 9 | from nav_msgs.msg import Path 10 | 11 | from geometry_msgs.msg import Pose 12 | from geometry_msgs.msg import PoseStamped 13 | from geometry_msgs.msg import Point 14 | import numpy as np 15 | import rospy 16 | 17 | from std_srvs.srv import Empty 18 | from std_srvs.srv import EmptyResponse 19 | from std_srvs.srv import EmptyRequest 20 | from quad_controller.srv import GetPath 21 | from quad_controller.srv import GetPathRequest 22 | from quad_controller.srv import SetPath 23 | from quad_controller.srv import SetPose 24 | 25 | from quad_controller import plotting_helpers 26 | 27 | class QuadPlotterNode(object): 28 | 29 | def __init__(self): 30 | 31 | self.trajectory = None 32 | self.waypoints = None 33 | self.path_history = list() 34 | self.running = False 35 | 36 | # source of data to keep track of the quads position and decide when to adjust controller goals 37 | self.pose_sub_ = rospy.Subscriber('/quad_rotor/pose', PoseStamped, self.pose_callback) 38 | 39 | # services for setting, and clearing the trajectory manager and runs state 40 | self.plot_grid_srv_ = rospy.Service('/quad_plotter/plot_grid',Empty, self.handle_plot_grid) 41 | self.plot_3d_srv_ = rospy.Service('/quad_plotter/plot_3d',Empty, self.handle_plot_3d) 42 | self.plot_one_srv_ = rospy.Service('/quad_plotter/plot_one',Empty, self.handle_plot_one) 43 | 44 | # services for setting, and clearing the trajectory manager and runs state 45 | self.start_recording_srv_ = rospy.Service('/quad_plotter/start_recording',Empty, self.handle_start_recording) 46 | self.stop_recording_srv_ = rospy.Service('/quad_plotter/stop_recording',Empty, self.handle_stop_recording) 47 | self.clear_path_history_srv_ = rospy.Service('/quad_plotter/clear_path_history',Empty, self.handle_clear_path_history) 48 | self.clear_waypoints_srv_ = rospy.Service('/quad_plotter/clear_waypoints',Empty, self.handle_clear_waypoints) 49 | 50 | self.load_waypoints_from_sim_srv_ = rospy.Service('/quad_plotter/load_waypoints_from_sim',Empty, self.handle_load_waypoints_from_sim) 51 | 52 | # services granting access to the quads history, trajectory, and waypoints 53 | self.get_path_history_srv_ = rospy.Service('/quad_plotter/get_path_history',GetPath, self.handle_get_path_history) 54 | 55 | def handle_load_waypoints_from_sim(self, empty): 56 | request = GetPathRequest() 57 | #rospy.wait_for_service('/quad_rotor/get_path') 58 | path_srv_prox_ = rospy.ServiceProxy('/quad_rotor/get_path', GetPath) 59 | response = path_srv_prox_(False) 60 | self.waypoints = path_response_to_array(response) 61 | return EmptyResponse() 62 | 63 | def handle_plot_one(self, empty): 64 | xyz_arr = pose_list_to_array(self.path_history) 65 | plotting_helpers.plot_path_2d(xyz_arr, planned_path=self.trajectory, waypoints=self.waypoints) 66 | return EmptyResponse() 67 | 68 | def handle_plot_3d(self, empty): 69 | xyz_arr = pose_list_to_array(self.path_history) 70 | plotting_helpers.plot_path_3d(xyz_arr, planned_path=self.trajectory, waypoints=self.waypoints) 71 | return EmptyResponse() 72 | 73 | def handle_plot_grid(self, empty): 74 | xyz_arr = pose_list_to_array(self.path_history) 75 | plotting_helpers.plot_path_grid(xyz_arr, planned_path=self.trajectory, waypoints=self.waypoints) 76 | return EmptyResponse() 77 | 78 | def handle_get_path_history(self, empty): 79 | if not self.path_history: 80 | raise RuntimeError('path history is empty, use start_recording gather data') 81 | path = Path() 82 | path.poses = self.path_history 83 | return path 84 | 85 | def handle_start_recording(self, empty): 86 | self.running = True 87 | return EmptyResponse() 88 | 89 | def handle_stop_recording(self, empty): 90 | self.running = False 91 | return EmptyResponse() 92 | 93 | def handle_clear_path_history(self, empty): 94 | del self.path_history 95 | self.path_history = list() 96 | return EmptyResponse() 97 | 98 | def handle_clear_waypoints(self, empty): 99 | del self.waypoints 100 | self.waypoints = None 101 | return EmptyResponse() 102 | 103 | def pose_callback(self, pose): 104 | if not self.running: 105 | return 106 | 107 | self.path_history.append(pose) 108 | 109 | 110 | def path_response_to_array(get_path_response): 111 | """Extract x,y,z position from a GetPathResponse""" 112 | point_list = list() 113 | for pose in get_path_response.path.poses: 114 | arr = np.array([pose.pose.position.x, 115 | pose.pose.position.y, 116 | pose.pose.position.z]) 117 | 118 | point_list.append(arr) 119 | return np.array(point_list) 120 | 121 | 122 | def pose_list_to_array(poses): 123 | """convert poselist into an numpy 2d array""" 124 | tmp_list = list() 125 | for p in poses: 126 | tmp_list.append([p.pose.position.x, 127 | p.pose.position.y, 128 | p.pose.position.z, 129 | 130 | p.pose.orientation.x, 131 | p.pose.orientation.y, 132 | p.pose.orientation.z, 133 | p.pose.orientation.w, 134 | 135 | p.header.stamp.to_sec() 136 | ]) 137 | 138 | return np.array(tmp_list) 139 | 140 | 141 | if __name__ == "__main__": 142 | rospy.init_node('quad_plotter') 143 | 144 | try: 145 | rpc = QuadPlotterNode() 146 | rospy.spin() 147 | except rospy.ROSInterruptException: 148 | pass 149 | -------------------------------------------------------------------------------- /quad_controller/scripts/quaternion_to_euler_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2017 Udacity Inc. 4 | # All Rights Reserved. 5 | 6 | # Author: Brandon Kinman 7 | 8 | import rospy 9 | import tf 10 | 11 | from geometry_msgs.msg import PoseStamped 12 | from quad_controller.msg import EulerAngles 13 | from sensor_msgs.msg import Imu 14 | 15 | class QuaternionToEuler(): 16 | def __init__(self): 17 | self.imu_sub_ = rospy.Subscriber("/quad_rotor/imu", Imu, self.imu_callback) 18 | self.pose_sub_ = rospy.Subscriber("/quad_rotor/pose", PoseStamped, self.pose_callback) 19 | self.imu_euler_pub_ = rospy.Publisher("/quad_rotor/imu_euler_angles", EulerAngles, queue_size = 1) 20 | self.pose_euler_pub_ = rospy.Publisher("/quad_rotor/pose_euler_angles", EulerAngles, queue_size = 1) 21 | 22 | rate = rospy.Rate(10) 23 | while not rospy.is_shutdown(): 24 | rate.sleep() 25 | 26 | def imu_callback(self, msg): 27 | ea_msg = EulerAngles() 28 | 29 | ea_msg.header = msg.header 30 | 31 | (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([msg.orientation.x, 32 | msg.orientation.y, 33 | msg.orientation.z, 34 | msg.orientation.w]) 35 | ea_msg.roll = roll 36 | ea_msg.pitch = pitch 37 | ea_msg.yaw = yaw 38 | self.imu_euler_pub_.publish(ea_msg) 39 | 40 | def pose_callback(self, msg): 41 | ea_msg = EulerAngles() 42 | 43 | ea_msg.header = msg.header 44 | 45 | (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([msg.pose.orientation.x, 46 | msg.pose.orientation.y, 47 | msg.pose.orientation.z, 48 | msg.pose.orientation.w]) 49 | ea_msg.roll = roll 50 | ea_msg.pitch = pitch 51 | ea_msg.yaw = yaw 52 | self.pose_euler_pub_.publish(ea_msg) 53 | 54 | if __name__ == '__main__': 55 | rospy.init_node('quaternion_to_euler') 56 | 57 | try: 58 | q2e = QuaternionToEuler() 59 | except rospy.ROSInterruptException: 60 | pass 61 | -------------------------------------------------------------------------------- /quad_controller/scripts/setup_2d_hover_world.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosservice call /quad_rotor/camera_pose_type '{data: 1}' 4 | rosservice call /quad_rotor/gravity '{data: true}' 5 | rosservice call /quad_rotor/x_force_constrained '{data: true}' 6 | rosservice call /quad_rotor/y_force_constrained '{data: true}' 7 | rosservice call /quad_rotor/x_torque_constrained '{data: true}' 8 | rosservice call /quad_rotor/y_torque_constrained '{data: true}' 9 | rosservice call /quad_rotor/z_torque_constrained '{data: true}' 10 | -------------------------------------------------------------------------------- /quad_controller/scripts/setup_attitude_world.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosservice call /quad_rotor/gravity '{data: false}' 4 | rosservice call /quad_rotor/x_force_constrained '{data: true}' 5 | rosservice call /quad_rotor/y_force_constrained '{data: true}' 6 | rosservice call /quad_rotor/z_force_constrained '{data: true}' 7 | rosservice call /quad_rotor/x_torque_constrained '{data: false}' 8 | rosservice call /quad_rotor/y_torque_constrained '{data: false}' 9 | rosservice call /quad_rotor/z_torque_constrained '{data: false}' 10 | rosservice call /quad_rotor/set_pose '{pose:{ position:{x: 0.0, y: 0.0, z: 10.0}, orientation:{x: 0.0, y: 0.0, z: 0.0, w: 0.0}}}' 11 | -------------------------------------------------------------------------------- /quad_controller/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['quad_controller'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /quad_controller/src/quad_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/RoboND-Controls-Lab/5f9386cf5d5dff261a2a9b868f2be8f5639953b9/quad_controller/src/quad_controller/__init__.py -------------------------------------------------------------------------------- /quad_controller/src/quad_controller/pid_controller.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2017 Udacity Inc. 2 | # All Rights Reserved. 3 | 4 | # Author: Brandon Kinman 5 | 6 | 7 | class PIDController: 8 | def __init__(self, kp = 0.0, ki = 0.0, kd = 0.0, max_windup = 10): 9 | #TODO 10 | pass 11 | def reset(self): 12 | #TODO 13 | pass 14 | 15 | def setTarget(self, target): 16 | #TODO 17 | pass 18 | 19 | def setKP(self, kp): 20 | #TODO 21 | pass 22 | 23 | def setKI(self, ki): 24 | #TODO 25 | pass 26 | 27 | def setKD(self, kd): 28 | #TODO 29 | pass 30 | 31 | def setMaxWindup(self, max_windup): 32 | #TODO 33 | pass 34 | 35 | def update(self, measured_value, timestamp): 36 | #TODO 37 | pass 38 | 39 | 40 | -------------------------------------------------------------------------------- /quad_controller/src/quad_controller/plotting_helpers.py: -------------------------------------------------------------------------------- 1 | """helper functions for plotting the path of the quad copter,and 2 | comparing the path it took to the path generated by trajectory_manager""" 3 | 4 | # Author: Devin Anzelmo 5 | 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from matplotlib import patches 9 | from mpl_toolkits.mplot3d import Axes3D 10 | import os 11 | import time 12 | import rospkg 13 | 14 | 15 | 16 | def plot_path_2d(quad_path, 17 | planned_path=None, 18 | waypoints=None, 19 | h_axis='x', 20 | v_axis='y'): 21 | 22 | """ Generates and saves a 2D plot of the quad's path. 23 | 24 | Use this function to generate individual 2D plots of the quads path, and the planned path 25 | Use h_axis and v_axis to specify which two dimension to compare. 26 | 27 | Args: 28 | quad_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z quad positions 29 | planned_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z target positions 30 | waypoints (numpy.ndarray): A 2D numpy.ndarray of x,y,z positions 31 | out_fname (str): string path and file name to save the output to 32 | h_axis (str): One of 'x', 'y', or 'z. The 3d axis to use as horizontal in the 2d plot. 33 | v_axis (str): One of 'x', 'y', or 'z. The 3d axis to use as vertical in the 2d plot. 34 | 35 | Returns: None 36 | 37 | """ 38 | plt.style.use('ggplot') 39 | fig = plt.figure(figsize=(16, 10)) 40 | axes = fig.add_subplot(111) 41 | fig.suptitle('Quadrotor Path', weight='bold', fontsize=14) 42 | 43 | # generate the plot for this axes 44 | axes = path_2d(quad_path, axes, planned_path, waypoints, h_axis, v_axis) 45 | 46 | quad_patch = patches.Patch(color='k') 47 | handles = [quad_patch] 48 | labels = ['quadrotor path'] 49 | 50 | if planned_path is not None: 51 | planned_patch = patches.Patch(color='r') 52 | labels.append('planned_path') 53 | handles.append(planned_patch) 54 | 55 | if waypoints is not None: 56 | waypoints_patch = patches.Patch(color='b') 57 | labels.append('waypoints') 58 | handles.append(waypoints_patch) 59 | 60 | fig.legend(handles=handles, 61 | labels=labels, 62 | loc='upper right', 63 | fontsize=11, 64 | frameon=False) 65 | 66 | rospack = rospkg.rospack.RosPack() 67 | base_path = rospack.get_path('quad_controller') 68 | 69 | out_path = os.path.join(base_path, 'output_data', 'one_plot_' + str(time.time()) + '.png') 70 | fig.savefig(out_path, dpi='figure') 71 | 72 | 73 | def plot_path_3d(quad_path, 74 | planned_path=None, 75 | waypoints=None, 76 | out_fname='/home/d/Desktop/tmp_3d.png', 77 | azimuth=210.0, 78 | elevation=45.0): 79 | 80 | """ Generates, displays, and saves a 3d plot of the quads path, 81 | 82 | Use this function to generate 3d plots of the path the quad took and path it was trying to follow. 83 | Use the parameters azimuth, and elevation to specify the initial viewing angle if saving plots to .png. 84 | 85 | Args: 86 | quad_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z quad positions 87 | planned_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z target positions 88 | waypoints (numpy.ndarray): A 2D numpy.ndarray of x,y,z positions 89 | out_fname (str): string path and file name to save the output to 90 | azimuth (float): The initial azimuth angle of the viewpoint for the matplotlib 3d plot 91 | elevation (float): the initial elevation of the viewpoint 92 | 93 | Returns: None 94 | 95 | """ 96 | 97 | plt.style.use('ggplot') 98 | fig = plt.figure(figsize=(16, 10)) 99 | fig.suptitle('Quadrotor Path', weight='bold', fontsize=14) 100 | ax3d = fig.add_subplot(111, projection='3d') 101 | 102 | ax3d = path_3d(quad_path, ax3d, planned_path, waypoints) 103 | ax3d.view_init(elevation, azimuth) 104 | 105 | quad_patch = patches.Patch(color='k') 106 | handles = [quad_patch] 107 | labels = ['quadrotor path'] 108 | 109 | if planned_path is not None: 110 | planned_patch = patches.Patch(color='r') 111 | labels.append('planned_path') 112 | handles.append(planned_patch) 113 | 114 | if waypoints is not None: 115 | waypoints_patch = patches.Patch(color='b') 116 | labels.append('waypoints') 117 | handles.append(waypoints_patch) 118 | 119 | fig.legend(handles=handles, 120 | labels=labels, 121 | loc='upper right', 122 | fontsize=11, 123 | frameon=False) 124 | 125 | 126 | rospack = rospkg.rospack.RosPack() 127 | base_path = rospack.get_path('quad_controller') 128 | 129 | out_path = os.path.join(base_path, 'output_data', 'isometric_plot_' + str(time.time()) + '.png') 130 | fig.savefig(out_path, dpi='figure') 131 | 132 | 133 | def plot_path_grid(quad_path, 134 | planned_path=None, 135 | waypoints=None, 136 | out_fname='/home/d/Desktop/tmp_grid.png', 137 | azimuth=210, 138 | elevation=45): 139 | """Generates and saves a two by two grid of plots of the quadrotor's path. 140 | 141 | Creates four plots with different perspectives on the quads path. Two perspectives with z on the horizontal axis, and the x,y plane. 142 | Additionally it generates a 3d plot with configurable angle. Shows the plot as well as saves it to .png 143 | 144 | Args: 145 | quad_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z quad positions 146 | planned_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z target positions 147 | waypoints (numpy.ndarray): A 2D numpy.ndaray of x,y,z positions 148 | out_fname (str): path and file name to save the output to 149 | azimuth (float): The initial azimuth angle of the viewpoint for the matplotlib 3d plot 150 | elevation (float): the initial elevation of the viewpoint 151 | 152 | Returns: None 153 | 154 | """ 155 | 156 | 157 | # set up the figure 158 | plt.style.use('ggplot') 159 | fig = plt.figure(figsize=(16,10)) 160 | fig.suptitle('Quadrotor Path: multiple perspectives', weight='bold', fontsize=14) 161 | 162 | # create the axes for the plot 163 | axe00 = fig.add_subplot(2,2,1) 164 | axe01 = fig.add_subplot(2,2,2, projection='3d') 165 | axe10 = fig.add_subplot(2,2,3) 166 | axe11 = fig.add_subplot(2,2,4) 167 | 168 | # generate the path subplots 169 | axe00 = path_2d(quad_path, axe00, planned_path, waypoints, h_axis='x', v_axis='y') 170 | axe01 = path_3d(quad_path, axe01, planned_path, waypoints) 171 | axe10 = path_2d(quad_path, axe10, planned_path, waypoints, h_axis='x', v_axis='z') 172 | axe11 = path_2d(quad_path, axe11, planned_path, waypoints, h_axis='y', v_axis='z') 173 | 174 | # set viewpoint for 3d 175 | axe01.view_init(elevation, azimuth) 176 | 177 | # add legend info to the plots 178 | quad_patch = patches.Patch(color='k') 179 | 180 | handles = [quad_patch] 181 | labels = ['quadrotor path'] 182 | 183 | if planned_path is not None: 184 | planned_patch = patches.Patch(color='r') 185 | labels.append('planned path') 186 | handles.append(planned_patch) 187 | 188 | if waypoints is not None: 189 | waypoints_patch = patches.Patch(color='b') 190 | labels.append('waypoints') 191 | handles.append(waypoints_patch) 192 | 193 | fig.legend(handles=handles, 194 | labels=labels, 195 | loc='upper right', 196 | fontsize=11, 197 | frameon=False) 198 | 199 | # save and show the plot 200 | rospack = rospkg.rospack.RosPack() 201 | base_path = rospack.get_path('quad_controller') 202 | 203 | out_path = os.path.join(base_path, 'output_data', 'grid_plot_' + str(time.time()) + '.png') 204 | 205 | fig.savefig(out_path, dpi='figure') 206 | 207 | 208 | def path_2d(quad_path, 209 | axes, 210 | planned_path=None, 211 | waypoints=None, 212 | h_axis='x', 213 | v_axis='y', 214 | quad_lw=1.0, 215 | path_size=3.5, 216 | wp_size=6.0): 217 | 218 | """ Plots the path of a quad on a matplotlib axes. 219 | 220 | Only use if you want to modify the look of the plots, otherwise use plot_path_2d to generate plots. 221 | 222 | Args: 223 | quad_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z quad positions 224 | axes (matplotlib axes): The matplotlib axes object the path's are plotted on. 225 | planned_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z target positions 226 | waypoints (numpy.ndarray): A 2D numpy.ndarray of x,y,z positions 227 | h_axis (str): One of 'x', 'y', or 'z. The 3d axis to use as horizontal in the 2d plot. 228 | v_axis (str): One of 'x', 'y', or 'z. The 3d axis to use as vertical in the 2d plot. 229 | quad_lw (float): The float linewidth for the quads path 230 | path_size (float): The float markersize for the planned path markers 231 | wp_size (float): The float markersize for the waypoint markers 232 | 233 | Returns: 234 | axes (matplotlib axes): matplotlib axes with the path's plotted. 235 | 236 | """ 237 | 238 | axis_labels = dict() 239 | axis_labels[0] = 'X' 240 | axis_labels[1] = 'Y' 241 | axis_labels[2] = 'Z' 242 | 243 | axis_pos = dict() 244 | axis_pos['x'] = 0 245 | axis_pos['y'] = 1 246 | axis_pos['z'] = 2 247 | 248 | # select the correct column for the horizontal and vertical axis 249 | h_ax = axis_pos[h_axis] 250 | v_ax = axis_pos[v_axis] 251 | 252 | if planned_path is not None: 253 | axes.plot(planned_path[:,h_ax], planned_path[:,v_ax], 'go', 254 | color='r',markersize=path_size, markeredgecolor='r') 255 | 256 | 257 | axes.plot(quad_path[:,h_ax], quad_path[:,v_ax], 258 | color='k', linewidth=quad_lw) 259 | 260 | if waypoints is not None: 261 | axes.plot(waypoints[:,h_ax], waypoints[:,v_ax], 'gH', 262 | markersize=wp_size, color='b', markeredgecolor='b') 263 | 264 | axes.tick_params(labelsize=9) 265 | axes.set_xlabel(axis_labels[h_ax], weight='bold') 266 | axes.set_ylabel(axis_labels[v_ax], weight='bold') 267 | 268 | return axes 269 | 270 | 271 | def path_3d(quad_path, 272 | ax3d, 273 | planned_path=None, 274 | waypoints=None, 275 | quad_lw=1.0, 276 | path_size=3.5, 277 | wp_size=6.0): 278 | 279 | """ Plots the path of a quad on a matplotlib axes. 280 | 281 | Only use if you want to modify the look of the plots, otherwise use plot_path_3d to generate plots. 282 | 283 | Args: 284 | quad_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z quad positions 285 | ax3d (mplot3d axes3d): The mplot3d axes3d object the path's are plotted onto. 286 | planned_path (numpy.ndarray): A 2D numpy.ndarray of x,y,z target positions 287 | waypoints (numpy.ndarray): A 2D numpy.ndarray of x,y,z positions 288 | quad_lw (float): The float linewidth for the quads path 289 | path_size (float): The float markersize for the planned path markers 290 | wp_size (float): The float markersize for the waypoint markers 291 | 292 | Returns: 293 | ax3d (mplot3d axes3d): The mplot3d axes3d with path's plotted onto it. 294 | 295 | """ 296 | 297 | if planned_path is not None: 298 | ax3d.plot(planned_path[:,0], planned_path[:,1], planned_path[:,2], 'go', 299 | color='r', markersize=path_size, markeredgecolor='r') 300 | 301 | if waypoints is not None: 302 | ax3d.plot(waypoints[:,0], waypoints[:,1], waypoints[:,2], 'gH', 303 | color='b', markersize=wp_size, markeredgecolor='b') 304 | 305 | 306 | ax3d.plot(quad_path[:,0], quad_path[:,1], quad_path[:,2], 307 | color='k', linewidth=quad_lw) 308 | 309 | ax3d.tick_params(labelsize=8) 310 | ax3d.set_xlabel('X', weight='bold') 311 | ax3d.set_ylabel('Y', weight='bold') 312 | ax3d.set_zlabel('Z', weight='bold') 313 | return ax3d 314 | -------------------------------------------------------------------------------- /quad_controller/src/quad_controller/twiddle.py: -------------------------------------------------------------------------------- 1 | class Twiddle(): 2 | def __init__(self, algorithm, params): 3 | self.algorithm_ = algorithm 4 | self.params_ = params 5 | self.first_run_ = True 6 | self.best_err_ = 0.0 7 | self.dp_ = [.2] * len(params) 8 | self.iterations_ = 0 9 | 10 | 11 | def run(self): 12 | if self.first_run_ is True: 13 | self.best_err_ = self.algorithm_(self.params_) 14 | self.iterations_ += 1 15 | self.first_run_ = False 16 | return self.iterations_, self.params_, self.best_err_ 17 | 18 | for i in range(len(self.params_)): 19 | #Update parameter and run algorithm 20 | self.params_[i] += self.dp_[i] 21 | if self.params_[i] < 0: 22 | self.params_[i] = 0 23 | err = self.algorithm_(self.params_) 24 | 25 | if err < self.best_err_: 26 | #Looks good, let's increase the parameters a little bit 27 | self.best_err_ = err 28 | self.dp_[i] *= 1.1 29 | else: 30 | #The error actually got worse, decrease the parameter 31 | self.params_[i] -= 2 * self.dp_[i] 32 | if self.params_[i] < 0: 33 | self.params_[i] = 0 34 | err = self.algorithm_(self.params_) 35 | 36 | if err < self.best_err_: 37 | self.best_err_ = err 38 | self.dp_[i] *= 1.1 39 | else: 40 | self.params_[i] += self.dp_[i] 41 | if self.params_[i] < 0: 42 | self.params_[i] = 0 43 | self.dp_[i] *= 0.9 44 | self.iterations_ += 1 45 | return self.iterations_, self.params_, self.best_err_ 46 | 47 | 48 | -------------------------------------------------------------------------------- /quad_controller/src/quad_controller/utils/test_run.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | 3 | class TestRun: 4 | def __init__(self): 5 | self.sample_values_ = [] 6 | self.sample_times_ = [] 7 | 8 | def addSample(self, sample_value, timestamp): 9 | self.sample_times_.append(timestamp) 10 | self.sample_values_.append(sample_value) 11 | return True 12 | 13 | def reset(self): 14 | self.sample_values_ = [] 15 | self.sample_times_ = [] 16 | 17 | def plotRun(self, titles, axis_labels): 18 | fig = plt.figure(figsize=(12,3)) 19 | plt.subplot(111) 20 | plt.title(titles[0]) 21 | plt.xlabel(axis_labels[0]) 22 | plt.ylabel(axis_labels[1]) 23 | plt.plot(self.sample_times_, self.sample_values_) 24 | plt.show() 25 | 26 | 27 | if __name__ == '__main__': 28 | # Generate a bunch of fake samples and times 29 | import random 30 | run = TestRun() 31 | for i,j in enumerate(range(0,100)): 32 | run.addSample(j+random.uniform(0.0, 5.0), i) 33 | 34 | run.plotRun(['Silly Plot of Nothing'], ['Time', 'Random Signal']) -------------------------------------------------------------------------------- /quad_controller/srv/GetPath.srv: -------------------------------------------------------------------------------- 1 | bool ignoreMe 2 | --- 3 | nav_msgs/Path path -------------------------------------------------------------------------------- /quad_controller/srv/SetFloat.srv: -------------------------------------------------------------------------------- 1 | float32 data 2 | --- 3 | bool success 4 | float32 newData -------------------------------------------------------------------------------- /quad_controller/srv/SetInt.srv: -------------------------------------------------------------------------------- 1 | int32 data 2 | --- 3 | bool success 4 | int32 newData -------------------------------------------------------------------------------- /quad_controller/srv/SetPath.srv: -------------------------------------------------------------------------------- 1 | nav_msgs/Path path 2 | --- 3 | bool success -------------------------------------------------------------------------------- /quad_controller/srv/SetPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | --- 3 | bool success 4 | string message 5 | --------------------------------------------------------------------------------