├── .github └── workflows │ ├── build_test.yml │ └── check_style.yml ├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── Tools ├── check_code_format.sh └── fix_code_style.sh ├── controller_msgs ├── CMakeLists.txt ├── include │ └── dummy ├── msg │ └── FlatTarget.msg └── package.xml ├── dependencies.rosinstall ├── geometric_controller ├── CMakeLists.txt ├── README.md ├── cfg │ └── GeometricController.cfg ├── include │ └── geometric_controller │ │ ├── common.h │ │ ├── control.h │ │ ├── geometric_controller.h │ │ ├── jerk_tracking_control.h │ │ ├── nonlinear_attitude_control.h │ │ └── nonlinear_geometric_control.h ├── launch │ ├── config_file.rviz │ ├── mav_trajectory_track_circle.launch │ ├── multi_vehicle_config_file.rviz │ ├── sitl_multi_vehicle_track_circle.launch │ ├── sitl_trajectory_track_circle.launch │ ├── sitl_trajectory_track_lamniscate.launch │ └── trajectory_controller.launch ├── package.xml ├── src │ ├── geometric_controller.cpp │ ├── geometric_controller_node.cpp │ ├── jerk_tracking_control.cpp │ ├── nonlinear_attitude_control.cpp │ └── nonlinear_geometric_control.cpp └── test │ ├── geometric_controller-test.cpp │ ├── main.cpp │ └── test_example.cpp ├── mavros_controllers ├── CMakeLists.txt └── package.xml └── trajectory_publisher ├── CMakeLists.txt ├── README.md ├── include └── trajectory_publisher │ ├── polynomialtrajectory.h │ ├── shapetrajectory.h │ ├── trajectory.h │ └── trajectoryPublisher.h ├── launch ├── config_file.rviz ├── mav_trajectory_setpointraw.launch ├── sitl_trajectory_setpointraw.launch └── trajectory_publisher.launch ├── package.xml └── src ├── polynomialtrajectory.cpp ├── shapetrajectory.cpp ├── trajectory.cpp ├── trajectoryPublisher.cpp └── trajectoryPublisher_node.cpp /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | 2 | name: Build Test 3 | on: 4 | push: 5 | branches: 6 | - 'master' 7 | pull_request: 8 | branches: 9 | - '*' 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | config: 18 | - {rosdistro: 'melodic', container: 'px4io/px4-dev-ros-melodic:2020-08-14'} 19 | - {rosdistro: 'noetic', container: 'px4io/px4-dev-ros-noetic:2020-08-20'} 20 | container: ${{ matrix.config.container }} 21 | steps: 22 | - uses: actions/checkout@v1 23 | with: 24 | token: ${{ secrets.ACCESS_TOKEN }} 25 | github-token: ${{ secrets.GITHUB_TOKEN }} 26 | - name: release_build_test 27 | working-directory: 28 | run: | 29 | apt update 30 | apt install -y python3-wstool 31 | mkdir -p $HOME/catkin_ws/src; 32 | cd $HOME/catkin_ws 33 | catkin init 34 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 35 | catkin config --merge-devel 36 | cd $HOME/catkin_ws/src 37 | ln -s $GITHUB_WORKSPACE 38 | cd $HOME/catkin_ws 39 | wstool init src src/mavros_controllers/dependencies.rosinstall 40 | wstool update -t src -j4 41 | rosdep update 42 | rosdep install --from-paths src --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 43 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False 44 | catkin build -j$(nproc) -l$(nproc) geometric_controller trajectory_publisher 45 | - name: unit_tests 46 | working-directory: 47 | run: | 48 | cd $HOME/catkin_ws/src 49 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True 50 | catkin build geometric_controller trajectory_publisher --no-deps -v -i --catkin-make-args tests 51 | source $HOME/catkin_ws/devel/setup.bash 52 | status=0 && for f in $HOME/catkin_ws/devel/lib/*/*-test; do $f || exit 1; done 53 | shell: bash 54 | -------------------------------------------------------------------------------- /.github/workflows/check_style.yml: -------------------------------------------------------------------------------- 1 | name: Style Checks 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | branches: 9 | - '*' 10 | 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | container: 18 | - 'px4io/px4-dev-simulation-focal:2020-09-14' 19 | container: ${{ matrix.container }} 20 | steps: 21 | - uses: actions/checkout@v1 22 | - name: submodule update 23 | run: git submodule update --init --recursive 24 | - name: Install clang-format 25 | run: apt update && apt install -y clang-format-6.0 26 | - name: Check Code format 27 | working-directory: Tools 28 | run: | 29 | ./check_code_format.sh .. 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | 3 | # Artifacts from clion 4 | */cmake-build-debug/* 5 | 6 | #Artifacts from vscode 7 | .vscode/* 8 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018-2021, Jaeyoung Lim 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | format: 2 | Tools/fix_code_style.sh . 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mavros_controllers 2 | [![Build Test](https://github.com/Jaeyoung-Lim/mavros_controllers/workflows/Build%20Test/badge.svg)](https://github.com/Jaeyoung-Lim/mavros_controllers/actions?query=workflow%3A%22Build+Test%22) [![DOI](https://zenodo.org/badge/140596755.svg)](https://zenodo.org/badge/latestdoi/140596755) 3 | 4 | Controllers for controlling MAVs using the [mavros](https://github.com/mavlink/mavros) package in OFFBOARD mode. 5 | 6 | 7 | ## Overview 8 | The repository contains controllers for controlling MAVs using the mavros package. The following packages are included in the repo 9 | - geometric_controller: Trajectory tracking controller based on geometric control 10 | - controller_msgs: custom message definitions 11 | - trajectory_publisher: Node publishing setpoints as states from motion primitives / trajectories for the controller to follow. 12 | 13 | [![Multiple drone](https://user-images.githubusercontent.com/5248102/87020057-a3e25200-c1d3-11ea-9f76-cd010cb8329a.gif)](https://user-images.githubusercontent.com/5248102/87020057-a3e25200-c1d3-11ea-9f76-cd010cb8329a.gif) 14 | 15 | [![Hovering drone](https://img.youtube.com/vi/FRaPGjX9m-c/0.jpg)](https://youtu.be/FRaPGjX9m-c "Hovering done") 16 | 17 | [![Circular trajectory tracking](https://img.youtube.com/vi/IEyocdnlYw0/0.jpg)](https://youtu.be/IEyocdnlYw0 "Circular trajectory tracking") 18 | 19 | ## Getting Started 20 | ### Install PX4 SITL(Only to Simulate) 21 | Follow the instructions as shown in the [ROS with Gazebo Simulation PX4 Documentation](https://dev.px4.io/master/en/simulation/ros_interface.html) 22 | To check if the necessary environment is setup correctly, you can run the gazebo SITL using the following command 23 | 24 | ```bash 25 | cd 26 | DONT_RUN=1 make px4_sitl_default gazebo 27 | ``` 28 | To source the PX4 environment, run the following commands 29 | 30 | ```bash 31 | cd 32 | source ~/catkin_ws/devel/setup.bash # (optional) 33 | source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default 34 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) 35 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo 36 | ``` 37 | 38 | You can run the rest of the roslaunch files in the same terminal 39 | 40 | ```bash 41 | roslaunch px4 posix_sitl.launch 42 | ``` 43 | 44 | You will need to source the PX4 environment in every new terminal you open to launch mavros_controllers. 45 | 46 | ### Installing mavros_controllers 47 | 48 | Create a catkin workspace: 49 | 50 | This folder will probably be already created since the previous process would have created it. If it is not present, do: 51 | 52 | ```bash 53 | mkdir -p ~/catkin_ws/src 54 | cd ~/catkin_ws 55 | catkin init 56 | catkin config --merge-devel 57 | cd ~/catkin_ws/src 58 | wstool init 59 | ``` 60 | 61 | ###### Clone this repository 62 | 63 | ```bash 64 | cd ~/catkin_ws/src 65 | git clone https://github.com/Jaeyoung-Lim/mavros_controllers 66 | ``` 67 | 68 | Now continue either with wstool to automatically download dependencies or download them manually. 69 | 70 | ###### With wstool 71 | 72 | wstool automates the installation of dependencies and updates all packages. If you have no problem updating the packages required by mavros_controllers and/or any other packages, follow this procedure. If not, follow the next 'Manually Download dependencies and build' section. 73 | 74 | ```bash 75 | cd ~/catkin_ws 76 | wstool merge -t src src/mavros_controllers/dependencies.rosinstall 77 | wstool update -t src -j4 78 | rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO 79 | catkin build 80 | source ~/catkin_ws/devel/setup.bash 81 | ``` 82 | 83 | 84 | ###### Manually Download dependencies and build 85 | 86 | If you did not install with wstool, you need to manually download the dependencies: 87 | - [catkin_simple](https://github.com/catkin/catkin_simple) 88 | - [eigen_catkin](https://github.com/ethz-asl/eigen_catkin) 89 | - [mav_comm](https://github.com/ethz-asl/mav_comm) 90 | 91 | Do: 92 | 93 | ```bash 94 | cd ~/catkin_ws/src 95 | git clone https://github.com/catkin/catkin_simple 96 | git clone https://github.com/ethz-asl/eigen_catkin 97 | git clone https://github.com/ethz-asl/mav_comm 98 | ``` 99 | 100 | Build all the packages: 101 | 102 | ```bash 103 | cd ~/catkin_ws 104 | catkin build 105 | source ~/catkin_ws/devel/setup.bash 106 | ``` 107 | 108 | ## Running the code 109 | Remember to source the workspace `setup.bash` before sourcing the PX4 environment. 110 | ```bash 111 | cd 112 | source ~/catkin_ws/devel/setup.bash # (necessary) 113 | source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default 114 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) 115 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo 116 | ``` 117 | The following launch file enables the geometric controller to follow a circular trajectory 118 | 119 | ``` bash 120 | roslaunch geometric_controller sitl_trajectory_track_circle.launch 121 | ``` 122 | 123 | If the UAV does not takeoff, please open QGroundControl and enable virtual joystick as mentioned [here](https://docs.qgroundcontrol.com/master/en/SettingsView/VirtualJoystick.html) 124 | 125 | ## Nodes 126 | `mavros_controllers` include the following packages. 127 | ### geometric_controller 128 | 129 | The geometric controller publishes and subscribes the following topics. 130 | - Parameters 131 | - /geometric_controller/mavname (default: "iris") 132 | - /geometric_controller/ctrl_mode (default: MODE_BODYRATE) 133 | - /geometric_controller/enable_sim (default: true) 134 | - /geometric_controller/enable_gazebo_state (default: false) 135 | - /geometric_controller/max_acc (default: 7.0) 136 | - /geometric_controller/yaw_heading (default: 0.0) 137 | - /geometric_controller/drag_dx (default: 0.0) 138 | - /geometric_controller/drag_dy (default: 0.0) 139 | - /geometric_controller/drag_dz (default: 0.0) 140 | - /geometric_controller/attctrl_constant (default: 0.2) 141 | - /geometric_controller/normalizedthrust_constant (default: 0.1) 142 | 143 | - Published Topics 144 | - command/bodyrate_command ( [mavros_msgs/AttitudeTarget](http://docs.ros.org/api/mavros_msgs/html/msg/AttitudeTarget.html) ) 145 | - reference/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 146 | 147 | - Subscribed Topics 148 | - reference/setpoint ( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) 149 | - /mavros/state ( [mavros_msgs/State](http://docs.ros.org/api/mavros_msgs/html/msg/State.html) ) 150 | - /mavros/local_position/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 151 | - /gazebo/model_states( [gazebo_msgs/ModelStates](http://docs.ros.org/kinetic/api/gazebo_msgs/html/msg/ModelState.html) ) 152 | - /mavros/local_position/velocity( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) 153 | 154 | ### trajectory_publisher 155 | 156 | Trajectory publisher publishes continous trajectories to the trajectory_controller. 157 | - Parameters 158 | - /trajectory_publisher/initpos_x (default: 0.0) 159 | - /trajectory_publisher/initpos_y (default: 0.0) 160 | - /trajectory_publisher/initpos_z (default: 1.0) 161 | - /trajectory_publisher/updaterate (default: 0.01) 162 | - /trajectory_publisher/horizon (default: 1.0) 163 | - /trajectory_publisher/maxjerk (default: 10.0) 164 | - /trajectory_publisher/trajectory_type (default: 0) 165 | - /trajectory_publisher/number_of_primitives (default: 7) 166 | - /trajectory_publisher/shape_radius (default: 1.0) 167 | 168 | - Published Topics 169 | - reference/trajectory ( [nav_msgs/Path](http://docs.ros.org/kinetic/api/nav_msgs/html/msg/Path.html) ) 170 | - reference/setpoint ( [geometry_msgs/TwistStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Twist.html) ) 171 | 172 | - Subscribed Topics 173 | - /trajectory_publisher/motionselector ([std_msgs/int32](http://docs.ros.org/api/std_msgs/html/msg/Int32.html)); 174 | - /mavros/local_position/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 175 | - /mavros/local_position/velocity( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) 176 | 177 | ## Contact 178 | Jaeyoung Lim jalim@ethz.ch 179 | 180 | ## Citation 181 | In case you use this work as an academic context, please cite as the following. 182 | ``` 183 | @misc{jaeyoung_lim_2019_2619313, 184 | author = {Jaeyoung Lim}, 185 | title = {{mavros_controllers - Aggressive trajectory 186 | tracking using mavros for PX4 enabled vehicles}}, 187 | month = mar, 188 | year = 2019, 189 | doi = {10.5281/zenodo.2652888}, 190 | url = {https://doi.org/10.5281/zenodo.2652888} 191 | } 192 | ``` 193 | 194 | ## References 195 | [1] Lee, Taeyoung, Melvin Leoky, and N. Harris McClamroch. "Geometric tracking control of a quadrotor UAV on SE (3)." Decision and Control (CDC), 2010 49th IEEE Conference on. IEEE, 2010. 196 | 197 | [2] Faessler, Matthias, Antonio Franchi, and Davide Scaramuzza. "Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories." IEEE Robot. Autom. Lett 3.2 (2018): 620-626. 198 | 199 | ### Build issues: 200 | 201 | 202 | ###### catkin_simple() or eigen_catkin() not found 203 | 204 | This should not have happened if you clone the catkin_simple and eigen_catkin repositories. Try again: 205 | 206 | ```bash 207 | cd ~/catkin_ws/src 208 | git clone https://github.com/catkin/catkin_simple 209 | git clone https://github.com/ethz-asl/eigen_catkin 210 | cd ~/catkin_ws 211 | catkin build mavros_controllers 212 | source ~/catkin_ws/devel/setup.bash 213 | ``` 214 | 215 | - Refer to [this issue](https://github.com/Jaeyoung-Lim/mavros_controllers/issues/61). 216 | 217 | ###### iris.sdf model not found: 218 | 219 | Try: 220 | ```bash 221 | cd 222 | make px4_sitl_default sitl_gazebo 223 | ``` 224 | 225 | or refer to [this issue](https://github.com/PX4/Firmware/issues?utf8=%E2%9C%93&q=%2Firis%2Firis.sdf+) the [ROS with Gazebo Simulation PX4 Documentation](https://dev.px4.io/master/en/simulation/ros_interface.html). 226 | -------------------------------------------------------------------------------- /Tools/check_code_format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Fix style recursively in all the repository 4 | sh fix_code_style.sh .. 5 | 6 | # Print the diff with the remote branch (empty if no diff) 7 | git --no-pager diff -U0 --color 8 | 9 | # Check if there are changes, and failed 10 | if ! git diff-index --quiet HEAD --; then echo "Code style check failed, please run clang-format (e.g. with scripts/fix_code_style.sh)"; exit 1; fi 11 | -------------------------------------------------------------------------------- /Tools/fix_code_style.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | STYLE="google" 4 | 5 | if [ "$#" -eq 0 ]; then 6 | echo "Usage: $0 " 7 | echo "" 8 | echo "ERROR: At least one source file or one directory must be provided!" 9 | 10 | exit 1 11 | fi 12 | 13 | for arg in "$@" 14 | do 15 | if [ -f $arg ]; then 16 | clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg 17 | elif [ -d $arg ]; then 18 | find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' 19 | find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs chmod 644 20 | fi 21 | done 22 | -------------------------------------------------------------------------------- /controller_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(controller_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs message_generation std_msgs) 5 | 6 | include_directories(include) 7 | 8 | add_message_files( 9 | DIRECTORY msg 10 | FILES 11 | FlatTarget.msg 12 | ) 13 | 14 | generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | CATKIN_DEPENDS geometry_msgs sensor_msgs message_runtime std_msgs) -------------------------------------------------------------------------------- /controller_msgs/include/dummy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/mavros_controllers/8b3fff0327b56c415aa24708bea5f37d76307404/controller_msgs/include/dummy -------------------------------------------------------------------------------- /controller_msgs/msg/FlatTarget.msg: -------------------------------------------------------------------------------- 1 | # reference for polynomial trajectory tracking 2 | # 3 | 4 | std_msgs/Header header 5 | 6 | uint8 type_mask 7 | uint8 IGNORE_SNAP = 1 # Position Velocity Acceleration Jerk Reference 8 | uint8 IGNORE_SNAP_JERK = 2 # Position Velocity Acceleration Reference 9 | uint8 IGNORE_SNAP_JERK_ACC = 4 # Position Reference 10 | 11 | geometry_msgs/Vector3 position 12 | geometry_msgs/Vector3 velocity 13 | geometry_msgs/Vector3 acceleration 14 | geometry_msgs/Vector3 jerk 15 | geometry_msgs/Vector3 snap -------------------------------------------------------------------------------- /controller_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | controller_msgs 4 | 0.0.1 5 | 6 | controller_msgs includes the definition for custom messages for the mavros_controllers package. 7 | 8 | 9 | Jaeyoung Lim 10 | Jaeyoung Lim 11 | BSD 12 | 13 | https://github.com/Jaeyoung-Lim/mavros_controllers.git 14 | https://github.com/Jaeyoung-Lim/mavros_controllers.git 15 | https://github.com/Jaeyoung-Lim/mavros_controllers/issues 16 | 17 | catkin 18 | 19 | message_generation 20 | message_runtime 21 | std_msgs 22 | geometry_msgs 23 | mavros_msgs 24 | sensor_msgs 25 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple'} 2 | - git: {local-name: mav_comm, uri: 'https://github.com/ethz-asl/mav_comm'} 3 | - git: {local-name: eigen_catkin, uri: 'https://github.com/ethz-asl/eigen_catkin'} 4 | -------------------------------------------------------------------------------- /geometric_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(geometric_controller) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | dynamic_reconfigure 10 | tf 11 | mavros 12 | mavros_extras 13 | mavros_msgs 14 | mavlink 15 | ) 16 | 17 | generate_dynamic_reconfigure_options( 18 | cfg/GeometricController.cfg 19 | ) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES geometric_controller 24 | CATKIN_DEPENDS roscpp rospy std_msgs mavros_msgs geometry_msgs sensor_msgs tf 25 | ) 26 | 27 | ############# 28 | # LIBRARIES # 29 | ############# 30 | include_directories( 31 | include 32 | ${Boost_INCLUDE_DIR} 33 | ${catkin_INCLUDE_DIRS} 34 | ${Eigen_INCLUDE_DIRS} 35 | ) 36 | 37 | add_library(${PROJECT_NAME} 38 | src/geometric_controller.cpp 39 | src/nonlinear_attitude_control.cpp 40 | src/nonlinear_geometric_control.cpp 41 | src/jerk_tracking_control.cpp 42 | ) 43 | add_dependencies(geometric_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 44 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 45 | 46 | ############ 47 | # BINARIES # 48 | ############ 49 | add_executable(geometric_controller_node 50 | src/geometric_controller_node.cpp 51 | ) 52 | add_dependencies(geometric_controller_node geometric_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 53 | target_link_libraries(geometric_controller_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 54 | ########## 55 | # EXPORT # 56 | ########## 57 | # cs_install() 58 | # cs_export() 59 | ############# 60 | ## Testing ## 61 | ############# 62 | 63 | if(CATKIN_ENABLE_TESTING) 64 | # Add gtest based cpp test target and link libraries 65 | catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp 66 | test/test_example.cpp 67 | test/geometric_controller-test.cpp) 68 | 69 | if(TARGET ${PROJECT_NAME}-test) 70 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} 71 | ${catkin_LIBRARIES} 72 | ${YAML_CPP_LIBRARIES}) 73 | endif() 74 | 75 | endif() 76 | -------------------------------------------------------------------------------- /geometric_controller/README.md: -------------------------------------------------------------------------------- 1 | # geometric_controller 2 | 3 | Trajectory trackking controller using [mavros](https://github.com/mavlink/mavros) package in PX4 OFFBOARD mode. This is a implementation of a geometric controller and differentially flat system considering rotor drag 4 | 5 | ## Overview 6 | Trajectory trackking controller using [mavros](https://github.com/mavlink/mavros) package in PX4 OFFBOARD mode. This is a implementation of a geometric controller and differentially flat system considering rotor drag 7 | 8 | [![Hovering done](https://img.youtube.com/vi/FRaPGjX9m-c/0.jpg)](https://youtu.be/FRaPGjX9m-c "Hovering done") 9 | 10 | ## Parameters 11 | - /geometric_controller/mavname (default: "iris") 12 | - /geometric_controller/ctrl_mode (default: MODE_BODYRATE) 13 | - /geometric_controller/enable_sim (default: true) 14 | - /geometric_controller/enable_gazebo_state (default: false) 15 | - /geometric_controller/max_acc (default: 7.0) 16 | - /geometric_controller/yaw_heading (default: 0.0) 17 | - /geometric_controller/drag_dx (default: 0.0) 18 | - /geometric_controller/drag_dy (default: 0.0) 19 | - /geometric_controller/drag_dz (default: 0.0) 20 | - /geometric_controller/attctrl_constant (default: 0.2) 21 | - /geometric_controller/normalizedthrust_constant (default: 0.1) 22 | 23 | 24 | ## Topics 25 | 26 | The geometric controller publishes and subscribes the following topics. 27 | 28 | - Published Topics 29 | - command/bodyrate_command ( [mavros_msgs/AttitudeTarget](http://docs.ros.org/api/mavros_msgs/html/msg/AttitudeTarget.html) ) 30 | - reference/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 31 | 32 | - Subscribed Topics 33 | - reference/setpoint ( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) 34 | - /mavros/state ( [mavr0s_msgs/State](http://docs.ros.org/api/mavros_msgs/html/msg/State.html) ) 35 | - /mavros/local_position/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 36 | - /gazebo/model_states( [gazebo_msgs/ModelStates](http://docs.ros.org/kinetic/api/gazebo_msgs/html/msg/ModelState.html) ) 37 | - /mavros/local_position/velocity( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) 38 | 39 | 40 | ## References 41 | [1] Lee, Taeyoung, Melvin Leoky, and N. Harris McClamroch. "Geometric tracking control of a quadrotor UAV on SE (3)." Decision and Control (CDC), 2010 49th IEEE Conference on. IEEE, 2010. 42 | 43 | [2] Faessler, Matthias, Antonio Franchi, and Davide Scaramuzza. "Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories." IEEE Robot. Autom. Lett 3.2 (2018): 620-626. 44 | 45 | 46 | ## Contact 47 | Jaeyoung Lim jalim@student.ethz.ch -------------------------------------------------------------------------------- /geometric_controller/cfg/GeometricController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "geometric_controller" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # geometric_controller 9 | 10 | gen.add("max_acc", double_t, 0, "Max Feedback Acceleration", 7.0, 1.0, 8.0) 11 | gen.add("Kp_x", double_t, 0, "Proportional gain for X position error", 8.0, 0.10, 20.0) 12 | gen.add("Kp_y", double_t, 0, "Proportional gain for Y position error", 8.0, 0.1, 20.0) 13 | gen.add("Kp_z", double_t, 0, "Proportional gain for Z position error", 10.0, 0.1, 20.0) 14 | gen.add("Kv_x", double_t, 0, "Proportional gain for X velocity error", 1.5, 0.1, 20.0) 15 | gen.add("Kv_y", double_t, 0, "Proportional gain for Y velocity error", 1.5, 0.1, 20.0) 16 | gen.add("Kv_z", double_t, 0, "Proportional gain for Z velocity error", 3.3, 0.1, 20.0) 17 | 18 | exit(gen.generate(PACKAGE, "geometric_controller", "GeometricController")) 19 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/common.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Common library 35 | * 36 | * Common library for geometric controller 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef COMMON_H 42 | #define COMMON_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | static Eigen::Matrix3d matrix_hat(const Eigen::Vector3d &v) { 50 | Eigen::Matrix3d m; 51 | // Sanity checks on M 52 | m << 0.0, -v(2), v(1), v(2), 0.0, -v(0), -v(1), v(0), 0.0; 53 | return m; 54 | } 55 | 56 | static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m) { 57 | Eigen::Vector3d v; 58 | // TODO: Sanity checks if m is skew symmetric 59 | v << m(7), m(2), m(3); 60 | return v; 61 | } 62 | 63 | inline Eigen::Vector3d toEigen(const geometry_msgs::Point &p) { 64 | Eigen::Vector3d ev3(p.x, p.y, p.z); 65 | return ev3; 66 | } 67 | 68 | inline Eigen::Vector3d toEigen(const geometry_msgs::Vector3 &v3) { 69 | Eigen::Vector3d ev3(v3.x, v3.y, v3.z); 70 | return ev3; 71 | } 72 | 73 | inline Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p) { 74 | Eigen::Vector4d quat; 75 | quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2), 76 | p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0); 77 | return quat; 78 | } 79 | 80 | inline Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q) { 81 | Eigen::Matrix3d rotmat; 82 | rotmat << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3), 2 * q(1) * q(2) - 2 * q(0) * q(3), 83 | 2 * q(0) * q(2) + 2 * q(1) * q(3), 84 | 85 | 2 * q(0) * q(3) + 2 * q(1) * q(2), q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3), 86 | 2 * q(2) * q(3) - 2 * q(0) * q(1), 87 | 88 | 2 * q(1) * q(3) - 2 * q(0) * q(2), 2 * q(0) * q(1) + 2 * q(2) * q(3), 89 | q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3); 90 | return rotmat; 91 | } 92 | 93 | inline Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R) { 94 | Eigen::Vector4d quat; 95 | double tr = R.trace(); 96 | if (tr > 0.0) { 97 | double S = sqrt(tr + 1.0) * 2.0; // S=4*qw 98 | quat(0) = 0.25 * S; 99 | quat(1) = (R(2, 1) - R(1, 2)) / S; 100 | quat(2) = (R(0, 2) - R(2, 0)) / S; 101 | quat(3) = (R(1, 0) - R(0, 1)) / S; 102 | } else if ((R(0, 0) > R(1, 1)) & (R(0, 0) > R(2, 2))) { 103 | double S = sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2.0; // S=4*qx 104 | quat(0) = (R(2, 1) - R(1, 2)) / S; 105 | quat(1) = 0.25 * S; 106 | quat(2) = (R(0, 1) + R(1, 0)) / S; 107 | quat(3) = (R(0, 2) + R(2, 0)) / S; 108 | } else if (R(1, 1) > R(2, 2)) { 109 | double S = sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)) * 2.0; // S=4*qy 110 | quat(0) = (R(0, 2) - R(2, 0)) / S; 111 | quat(1) = (R(0, 1) + R(1, 0)) / S; 112 | quat(2) = 0.25 * S; 113 | quat(3) = (R(1, 2) + R(2, 1)) / S; 114 | } else { 115 | double S = sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)) * 2.0; // S=4*qz 116 | quat(0) = (R(1, 0) - R(0, 1)) / S; 117 | quat(1) = (R(0, 2) + R(2, 0)) / S; 118 | quat(2) = (R(1, 2) + R(2, 1)) / S; 119 | quat(3) = 0.25 * S; 120 | } 121 | return quat; 122 | } 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef CONTROL_H 42 | #define CONTROL_H 43 | 44 | #include 45 | 46 | class Control { 47 | public: 48 | Control(){}; 49 | virtual ~Control(){}; 50 | virtual void Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, 51 | const Eigen::Vector3d &ref_jerk){}; 52 | Eigen::Vector3d getDesiredThrust() { return desired_thrust_; }; 53 | Eigen::Vector3d getDesiredRate() { return desired_rate_; }; 54 | Eigen::Vector3d desired_rate_{Eigen::Vector3d::Zero()}; 55 | Eigen::Vector3d desired_thrust_{Eigen::Vector3d::Zero()}; 56 | 57 | protected: 58 | private: 59 | }; 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/geometric_controller.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Geometric Controller 35 | * 36 | * Geometric controller 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef GEOMETRIC_CONTROLLER_H 42 | #define GEOMETRIC_CONTROLLER_H 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | 73 | #include "geometric_controller/common.h" 74 | #include "geometric_controller/control.h" 75 | 76 | #define ERROR_QUATERNION 1 77 | #define ERROR_GEOMETRIC 2 78 | 79 | using namespace std; 80 | using namespace Eigen; 81 | 82 | enum class MAV_STATE { 83 | MAV_STATE_UNINIT, 84 | MAV_STATE_BOOT, 85 | MAV_STATE_CALIBRATIN, 86 | MAV_STATE_STANDBY, 87 | MAV_STATE_ACTIVE, 88 | MAV_STATE_CRITICAL, 89 | MAV_STATE_EMERGENCY, 90 | MAV_STATE_POWEROFF, 91 | MAV_STATE_FLIGHT_TERMINATION, 92 | }; 93 | 94 | class geometricCtrl { 95 | private: 96 | ros::NodeHandle nh_; 97 | ros::NodeHandle nh_private_; 98 | ros::Subscriber referenceSub_; 99 | ros::Subscriber flatreferenceSub_; 100 | ros::Subscriber multiDOFJointSub_; 101 | ros::Subscriber mavstateSub_; 102 | ros::Subscriber mavposeSub_, gzmavposeSub_; 103 | ros::Subscriber mavtwistSub_; 104 | ros::Subscriber yawreferenceSub_; 105 | ros::Publisher rotorVelPub_, angularVelPub_, target_pose_pub_; 106 | ros::Publisher referencePosePub_; 107 | ros::Publisher posehistoryPub_; 108 | ros::Publisher systemstatusPub_; 109 | ros::ServiceClient arming_client_; 110 | ros::ServiceClient set_mode_client_; 111 | ros::ServiceServer ctrltriggerServ_; 112 | ros::ServiceServer land_service_; 113 | ros::Timer cmdloop_timer_, statusloop_timer_; 114 | ros::Time last_request_, reference_request_now_, reference_request_last_; 115 | 116 | string mav_name_; 117 | bool fail_detec_{false}; 118 | bool feedthrough_enable_{false}; 119 | bool ctrl_enable_{true}; 120 | int ctrl_mode_; 121 | bool landing_commanded_{false}; 122 | bool sim_enable_; 123 | bool velocity_yaw_; 124 | double kp_rot_, kd_rot_; 125 | double reference_request_dt_; 126 | double norm_thrust_const_, norm_thrust_offset_; 127 | double max_fb_acc_; 128 | 129 | mavros_msgs::State current_state_; 130 | mavros_msgs::CommandBool arm_cmd_; 131 | std::vector posehistory_vector_; 132 | MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_ACTIVE; 133 | 134 | double initTargetPos_x_, initTargetPos_y_, initTargetPos_z_; 135 | Eigen::Vector3d targetPos_, targetVel_, targetAcc_, targetJerk_, targetSnap_, targetPos_prev_, targetVel_prev_; 136 | Eigen::Vector3d mavPos_, mavVel_, mavRate_; 137 | double mavYaw_; 138 | Eigen::Vector3d gravity_{Eigen::Vector3d(0.0, 0.0, -9.8)}; 139 | Eigen::Vector4d mavAtt_, q_des; 140 | Eigen::Vector4d cmdBodyRate_; //{wx, wy, wz, Thrust} 141 | Eigen::Vector3d Kpos_, Kvel_, D_; 142 | double Kpos_x_, Kpos_y_, Kpos_z_, Kvel_x_, Kvel_y_, Kvel_z_; 143 | int posehistory_window_; 144 | 145 | void pubMotorCommands(); 146 | void pubRateCommands(const Eigen::Vector4d &cmd, const Eigen::Vector4d &target_attitude); 147 | void pubReferencePose(const Eigen::Vector3d &target_position, const Eigen::Vector4d &target_attitude); 148 | void pubPoseHistory(); 149 | void pubSystemStatus(); 150 | void appendPoseHistory(); 151 | void odomCallback(const nav_msgs::OdometryConstPtr &odomMsg); 152 | void targetCallback(const geometry_msgs::TwistStamped &msg); 153 | void flattargetCallback(const controller_msgs::FlatTarget &msg); 154 | void yawtargetCallback(const std_msgs::Float32 &msg); 155 | void multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory &msg); 156 | void keyboardCallback(const geometry_msgs::Twist &msg); 157 | void cmdloopCallback(const ros::TimerEvent &event); 158 | void mavstateCallback(const mavros_msgs::State::ConstPtr &msg); 159 | void mavposeCallback(const geometry_msgs::PoseStamped &msg); 160 | void mavtwistCallback(const geometry_msgs::TwistStamped &msg); 161 | void statusloopCallback(const ros::TimerEvent &event); 162 | bool ctrltriggerCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); 163 | bool landCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); 164 | geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d &position, Eigen::Vector4d &orientation); 165 | void computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd, const Eigen::Vector3d &target_acc); 166 | Eigen::Vector3d controlPosition(const Eigen::Vector3d &target_pos, const Eigen::Vector3d &target_vel, 167 | const Eigen::Vector3d &target_acc); 168 | Eigen::Vector3d poscontroller(const Eigen::Vector3d &pos_error, const Eigen::Vector3d &vel_error); 169 | Eigen::Vector4d attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, 170 | Eigen::Vector4d &curr_att); 171 | 172 | enum FlightState { WAITING_FOR_HOME_POSE, MISSION_EXECUTION, LANDING, LANDED } node_state; 173 | 174 | template 175 | void waitForPredicate(const T *pred, const std::string &msg, double hz = 2.0) { 176 | ros::Rate pause(hz); 177 | ROS_INFO_STREAM(msg); 178 | while (ros::ok() && !(*pred)) { 179 | ros::spinOnce(); 180 | pause.sleep(); 181 | } 182 | }; 183 | geometry_msgs::Pose home_pose_; 184 | bool received_home_pose; 185 | std::shared_ptr controller_; 186 | 187 | public: 188 | void dynamicReconfigureCallback(geometric_controller::GeometricControllerConfig &config, uint32_t level); 189 | geometricCtrl(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 190 | virtual ~geometricCtrl(); 191 | void getStates(Eigen::Vector3d &pos, Eigen::Vector4d &att, Eigen::Vector3d &vel, Eigen::Vector3d &angvel) { 192 | pos = mavPos_; 193 | att = mavAtt_; 194 | vel = mavVel_; 195 | angvel = mavRate_; 196 | }; 197 | void getErrors(Eigen::Vector3d &pos, Eigen::Vector3d &vel) { 198 | pos = mavPos_ - targetPos_; 199 | vel = mavVel_ - targetVel_; 200 | }; 201 | void setBodyRateCommand(Eigen::Vector4d bodyrate_command) { cmdBodyRate_ = bodyrate_command; }; 202 | void setFeedthrough(bool feed_through) { feedthrough_enable_ = feed_through; }; 203 | void setDesiredAcceleration(Eigen::Vector3d &acceleration) { targetAcc_ = acceleration; }; 204 | static Eigen::Vector4d acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw); 205 | static double getVelocityYaw(const Eigen::Vector3d velocity) { return atan2(velocity(1), velocity(0)); }; 206 | }; 207 | 208 | #endif 209 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/jerk_tracking_control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef JERK_TRACKING_CONTROL_H 42 | #define JERK_TRACKING_CONTROL_H 43 | 44 | #include "geometric_controller/common.h" 45 | #include "geometric_controller/control.h" 46 | 47 | class JerkTrackingControl : public Control { 48 | public: 49 | JerkTrackingControl(); 50 | virtual ~JerkTrackingControl(); 51 | void Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, 52 | const Eigen::Vector3d &ref_jerk) override; 53 | 54 | private: 55 | Eigen::Vector3d last_ref_acc_{Eigen::Vector3d::Zero()}; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/nonlinear_attitude_control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef NONLINEAR_ATTITUDE_CONTROL_H 42 | #define NONLINEAR_ATTITUDE_CONTROL_H 43 | 44 | #include "geometric_controller/common.h" 45 | #include "geometric_controller/control.h" 46 | 47 | class NonlinearAttitudeControl : public Control { 48 | public: 49 | NonlinearAttitudeControl(double attctrl_tau); 50 | virtual ~NonlinearAttitudeControl(); 51 | void Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, 52 | const Eigen::Vector3d &ref_jerk) override; 53 | 54 | private: 55 | double attctrl_tau_{1.0}; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /geometric_controller/include/geometric_controller/nonlinear_geometric_control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #ifndef NONLINEAR_GEOMETRIC_CONTROL_H 42 | #define NONLINEAR_GEOMETRIC_CONTROL_H 43 | 44 | #include "geometric_controller/common.h" 45 | #include "geometric_controller/control.h" 46 | 47 | class NonlinearGeometricControl : public Control { 48 | public: 49 | NonlinearGeometricControl(double attctrl_tau); 50 | virtual ~NonlinearGeometricControl(); 51 | void Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, 52 | const Eigen::Vector3d &ref_jerk) override; 53 | 54 | private: 55 | double attctrl_tau_{1.0}; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /geometric_controller/launch/config_file.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Pose1 11 | - /Pose2 12 | - /Path1 13 | - /Path2 14 | Splitter Ratio: 0.5 15 | Tree Height: 768 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679016 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Axes Length: 1 58 | Axes Radius: 0.100000001 59 | Class: rviz/Pose 60 | Color: 255; 25; 0 61 | Enabled: true 62 | Head Length: 0.300000012 63 | Head Radius: 0.100000001 64 | Name: Pose 65 | Shaft Length: 1 66 | Shaft Radius: 0.0500000007 67 | Shape: Axes 68 | Topic: /mavros/local_position/pose 69 | Unreliable: false 70 | Value: true 71 | - Alpha: 1 72 | Axes Length: 0.5 73 | Axes Radius: 0.0500000007 74 | Class: rviz/Pose 75 | Color: 0; 255; 0 76 | Enabled: true 77 | Head Length: 0.300000012 78 | Head Radius: 0.100000001 79 | Name: Pose 80 | Shaft Length: 1 81 | Shaft Radius: 0.0500000007 82 | Shape: Axes 83 | Topic: /reference/pose 84 | Unreliable: false 85 | Value: true 86 | - Alpha: 1 87 | Buffer Length: 1 88 | Class: rviz/Path 89 | Color: 25; 255; 0 90 | Enabled: true 91 | Head Diameter: 0.300000012 92 | Head Length: 0.200000003 93 | Length: 0.300000012 94 | Line Style: Lines 95 | Line Width: 0.0299999993 96 | Name: Path 97 | Offset: 98 | X: 0 99 | Y: 0 100 | Z: 0 101 | Pose Color: 255; 85; 255 102 | Pose Style: None 103 | Radius: 0.0299999993 104 | Shaft Diameter: 0.100000001 105 | Shaft Length: 0.100000001 106 | Topic: /trajectory_publisher/trajectory 107 | Unreliable: false 108 | Value: true 109 | - Alpha: 1 110 | Buffer Length: 1 111 | Class: rviz/Path 112 | Color: 0; 0; 255 113 | Enabled: true 114 | Head Diameter: 0.300000012 115 | Head Length: 0.200000003 116 | Length: 0.300000012 117 | Line Style: Lines 118 | Line Width: 0.0299999993 119 | Name: Path 120 | Offset: 121 | X: 0 122 | Y: 0 123 | Z: 0 124 | Pose Color: 255; 85; 255 125 | Pose Style: None 126 | Radius: 0.0299999993 127 | Shaft Diameter: 0.100000001 128 | Shaft Length: 0.100000001 129 | Topic: /geometric_controller/path 130 | Unreliable: false 131 | Value: true 132 | Enabled: true 133 | Global Options: 134 | Background Color: 48; 48; 48 135 | Default Light: true 136 | Fixed Frame: map 137 | Frame Rate: 30 138 | Name: root 139 | Tools: 140 | - Class: rviz/Interact 141 | Hide Inactive Objects: true 142 | - Class: rviz/MoveCamera 143 | - Class: rviz/Select 144 | - Class: rviz/FocusCamera 145 | - Class: rviz/Measure 146 | - Class: rviz/SetInitialPose 147 | Topic: /initialpose 148 | - Class: rviz/SetGoal 149 | Topic: /move_base_simple/goal 150 | - Class: rviz/PublishPoint 151 | Single click: true 152 | Topic: /clicked_point 153 | Value: true 154 | Views: 155 | Current: 156 | Class: rviz/Orbit 157 | Distance: 5.8504715 158 | Enable Stereo Rendering: 159 | Stereo Eye Separation: 0.0599999987 160 | Stereo Focal Distance: 1 161 | Swap Stereo Eyes: false 162 | Value: false 163 | Focal Point: 164 | X: 0 165 | Y: 0 166 | Z: 0 167 | Focal Shape Fixed Size: true 168 | Focal Shape Size: 0.0500000007 169 | Invert Z Axis: false 170 | Name: Current View 171 | Near Clip Distance: 0.00999999978 172 | Pitch: 0.635397851 173 | Target Frame: 174 | Value: Orbit (rviz) 175 | Yaw: 0.720391929 176 | Saved: ~ 177 | Window Geometry: 178 | Displays: 179 | collapsed: false 180 | Height: 1056 181 | Hide Left Dock: false 182 | Hide Right Dock: false 183 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000388fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000388000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000388fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000003880000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000038800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 184 | Selection: 185 | collapsed: false 186 | Time: 187 | collapsed: false 188 | Tool Properties: 189 | collapsed: false 190 | Views: 191 | collapsed: false 192 | Width: 1920 193 | X: 65 194 | Y: 24 195 | -------------------------------------------------------------------------------- /geometric_controller/launch/mav_trajectory_track_circle.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 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /geometric_controller/launch/multi_vehicle_config_file.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 555 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Buffer Length: 1 57 | Class: rviz/Path 58 | Color: 255; 0; 0 59 | Enabled: true 60 | Head Diameter: 0.30000001192092896 61 | Head Length: 0.20000000298023224 62 | Length: 0.30000001192092896 63 | Line Style: Lines 64 | Line Width: 0.029999999329447746 65 | Name: UAV0 Current Path 66 | Offset: 67 | X: 0 68 | Y: 0 69 | Z: 0 70 | Pose Color: 255; 85; 255 71 | Pose Style: None 72 | Radius: 0.029999999329447746 73 | Shaft Diameter: 0.10000000149011612 74 | Shaft Length: 0.10000000149011612 75 | Topic: /uav0/geometric_controller/path 76 | Unreliable: false 77 | Value: true 78 | - Alpha: 1 79 | Buffer Length: 1 80 | Class: rviz/Path 81 | Color: 25; 255; 0 82 | Enabled: true 83 | Head Diameter: 0.30000001192092896 84 | Head Length: 0.20000000298023224 85 | Length: 0.30000001192092896 86 | Line Style: Lines 87 | Line Width: 0.029999999329447746 88 | Name: UAV0 Reference Path 89 | Offset: 90 | X: 0 91 | Y: 0 92 | Z: 0 93 | Pose Color: 255; 85; 255 94 | Pose Style: None 95 | Radius: 0.029999999329447746 96 | Shaft Diameter: 0.10000000149011612 97 | Shaft Length: 0.10000000149011612 98 | Topic: /uav0/trajectory_publisher/trajectory 99 | Unreliable: false 100 | Value: true 101 | - Alpha: 1 102 | Axes Length: 1 103 | Axes Radius: 0.10000000149011612 104 | Class: rviz/Pose 105 | Color: 255; 25; 0 106 | Enabled: true 107 | Head Length: 0.30000001192092896 108 | Head Radius: 0.10000000149011612 109 | Name: UAV0 Pose 110 | Shaft Length: 1 111 | Shaft Radius: 0.05000000074505806 112 | Shape: Axes 113 | Topic: /uav0/mavros/local_position/pose 114 | Unreliable: false 115 | Value: true 116 | - Alpha: 1 117 | Buffer Length: 1 118 | Class: rviz/Path 119 | Color: 255; 0; 0 120 | Enabled: true 121 | Head Diameter: 0.30000001192092896 122 | Head Length: 0.20000000298023224 123 | Length: 0.30000001192092896 124 | Line Style: Lines 125 | Line Width: 0.029999999329447746 126 | Name: UAV1 Current Path 127 | Offset: 128 | X: 0 129 | Y: 0 130 | Z: 0 131 | Pose Color: 255; 85; 255 132 | Pose Style: None 133 | Radius: 0.029999999329447746 134 | Shaft Diameter: 0.10000000149011612 135 | Shaft Length: 0.10000000149011612 136 | Topic: /uav1/geometric_controller/path 137 | Unreliable: false 138 | Value: true 139 | - Alpha: 1 140 | Axes Length: 1 141 | Axes Radius: 0.10000000149011612 142 | Class: rviz/Pose 143 | Color: 255; 25; 0 144 | Enabled: true 145 | Head Length: 0.30000001192092896 146 | Head Radius: 0.10000000149011612 147 | Name: UAV1 Pose 148 | Shaft Length: 1 149 | Shaft Radius: 0.05000000074505806 150 | Shape: Axes 151 | Topic: /uav1/mavros/local_position/pose 152 | Unreliable: false 153 | Value: true 154 | - Alpha: 1 155 | Buffer Length: 1 156 | Class: rviz/Path 157 | Color: 25; 255; 0 158 | Enabled: true 159 | Head Diameter: 0.30000001192092896 160 | Head Length: 0.20000000298023224 161 | Length: 0.30000001192092896 162 | Line Style: Lines 163 | Line Width: 0.029999999329447746 164 | Name: UAV1 Reference Path 165 | Offset: 166 | X: 0 167 | Y: 0 168 | Z: 0 169 | Pose Color: 255; 85; 255 170 | Pose Style: None 171 | Radius: 0.029999999329447746 172 | Shaft Diameter: 0.10000000149011612 173 | Shaft Length: 0.10000000149011612 174 | Topic: /uav1/trajectory_publisher/trajectory 175 | Unreliable: false 176 | Value: true 177 | Enabled: true 178 | Global Options: 179 | Background Color: 48; 48; 48 180 | Default Light: true 181 | Fixed Frame: map 182 | Frame Rate: 30 183 | Name: root 184 | Tools: 185 | - Class: rviz/Interact 186 | Hide Inactive Objects: true 187 | - Class: rviz/MoveCamera 188 | - Class: rviz/Select 189 | - Class: rviz/FocusCamera 190 | - Class: rviz/Measure 191 | - Class: rviz/SetInitialPose 192 | Theta std deviation: 0.2617993950843811 193 | Topic: /initialpose 194 | X std deviation: 0.5 195 | Y std deviation: 0.5 196 | - Class: rviz/SetGoal 197 | Topic: /move_base_simple/goal 198 | - Class: rviz/PublishPoint 199 | Single click: true 200 | Topic: /clicked_point 201 | Value: true 202 | Views: 203 | Current: 204 | Class: rviz/Orbit 205 | Distance: 10 206 | Enable Stereo Rendering: 207 | Stereo Eye Separation: 0.05999999865889549 208 | Stereo Focal Distance: 1 209 | Swap Stereo Eyes: false 210 | Value: false 211 | Focal Point: 212 | X: 0 213 | Y: 0 214 | Z: 0 215 | Focal Shape Fixed Size: true 216 | Focal Shape Size: 0.05000000074505806 217 | Invert Z Axis: false 218 | Name: Current View 219 | Near Clip Distance: 0.009999999776482582 220 | Pitch: 0.5753982067108154 221 | Target Frame: 222 | Value: Orbit (rviz) 223 | Yaw: 0.17039847373962402 224 | Saved: ~ 225 | Window Geometry: 226 | Displays: 227 | collapsed: false 228 | Height: 846 229 | Hide Left Dock: false 230 | Hide Right Dock: false 231 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 232 | Selection: 233 | collapsed: false 234 | Time: 235 | collapsed: false 236 | Tool Properties: 237 | collapsed: false 238 | Views: 239 | collapsed: false 240 | Width: 1200 241 | X: 67 242 | Y: 30 243 | -------------------------------------------------------------------------------- /geometric_controller/launch/sitl_multi_vehicle_track_circle.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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /geometric_controller/launch/sitl_trajectory_track_circle.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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /geometric_controller/launch/sitl_trajectory_track_lamniscate.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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /geometric_controller/launch/trajectory_controller.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 | -------------------------------------------------------------------------------- /geometric_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | geometric_controller 4 | 0.0.0 5 | Polynomial Trajectory Tracking controller using a geometric controller 6 | 7 | Jaeyoung Lim 8 | 9 | BSD 10 | 11 | 12 | dynamic_reconfigure 13 | catkin 14 | roscpp 15 | rospy 16 | std_msgs 17 | sensor_msgs 18 | nav_msgs 19 | geometry_msgs 20 | controller_msgs 21 | tf 22 | mavros 23 | mavros_extras 24 | mavros_msgs 25 | mavros_extras 26 | eigen_catkin 27 | dynamic_reconfigure 28 | roscpp 29 | rospy 30 | std_msgs 31 | sensor_msgs 32 | geometry_msgs 33 | controller_msgs 34 | nav_msgs 35 | tf 36 | mavros 37 | mavros_extras 38 | mavros_msgs 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /geometric_controller/src/geometric_controller.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Geometric Controller 35 | * 36 | * Geometric controller 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #include "geometric_controller/geometric_controller.h" 42 | #include "geometric_controller/jerk_tracking_control.h" 43 | #include "geometric_controller/nonlinear_attitude_control.h" 44 | #include "geometric_controller/nonlinear_geometric_control.h" 45 | 46 | using namespace Eigen; 47 | using namespace std; 48 | // Constructor 49 | geometricCtrl::geometricCtrl(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) 50 | : nh_(nh), nh_private_(nh_private), node_state(WAITING_FOR_HOME_POSE) { 51 | referenceSub_ = 52 | nh_.subscribe("reference/setpoint", 1, &geometricCtrl::targetCallback, this, ros::TransportHints().tcpNoDelay()); 53 | flatreferenceSub_ = nh_.subscribe("reference/flatsetpoint", 1, &geometricCtrl::flattargetCallback, this, 54 | ros::TransportHints().tcpNoDelay()); 55 | yawreferenceSub_ = 56 | nh_.subscribe("reference/yaw", 1, &geometricCtrl::yawtargetCallback, this, ros::TransportHints().tcpNoDelay()); 57 | multiDOFJointSub_ = nh_.subscribe("command/trajectory", 1, &geometricCtrl::multiDOFJointCallback, this, 58 | ros::TransportHints().tcpNoDelay()); 59 | mavstateSub_ = 60 | nh_.subscribe("mavros/state", 1, &geometricCtrl::mavstateCallback, this, ros::TransportHints().tcpNoDelay()); 61 | mavposeSub_ = nh_.subscribe("mavros/local_position/pose", 1, &geometricCtrl::mavposeCallback, this, 62 | ros::TransportHints().tcpNoDelay()); 63 | mavtwistSub_ = nh_.subscribe("mavros/local_position/velocity_local", 1, &geometricCtrl::mavtwistCallback, this, 64 | ros::TransportHints().tcpNoDelay()); 65 | ctrltriggerServ_ = nh_.advertiseService("trigger_rlcontroller", &geometricCtrl::ctrltriggerCallback, this); 66 | cmdloop_timer_ = nh_.createTimer(ros::Duration(0.01), &geometricCtrl::cmdloopCallback, 67 | this); // Define timer for constant loop rate 68 | statusloop_timer_ = nh_.createTimer(ros::Duration(1), &geometricCtrl::statusloopCallback, 69 | this); // Define timer for constant loop rate 70 | 71 | angularVelPub_ = nh_.advertise("command/bodyrate_command", 1); 72 | referencePosePub_ = nh_.advertise("reference/pose", 1); 73 | target_pose_pub_ = nh_.advertise("mavros/setpoint_position/local", 10); 74 | posehistoryPub_ = nh_.advertise("geometric_controller/path", 10); 75 | systemstatusPub_ = nh_.advertise("mavros/companion_process/status", 1); 76 | arming_client_ = nh_.serviceClient("mavros/cmd/arming"); 77 | set_mode_client_ = nh_.serviceClient("mavros/set_mode"); 78 | land_service_ = nh_.advertiseService("land", &geometricCtrl::landCallback, this); 79 | 80 | nh_private_.param("mavname", mav_name_, "iris"); 81 | nh_private_.param("ctrl_mode", ctrl_mode_, ERROR_QUATERNION); 82 | nh_private_.param("enable_sim", sim_enable_, true); 83 | nh_private_.param("velocity_yaw", velocity_yaw_, false); 84 | nh_private_.param("max_acc", max_fb_acc_, 9.0); 85 | nh_private_.param("yaw_heading", mavYaw_, 0.0); 86 | 87 | double dx, dy, dz; 88 | nh_private_.param("drag_dx", dx, 0.0); 89 | nh_private_.param("drag_dy", dy, 0.0); 90 | nh_private_.param("drag_dz", dz, 0.0); 91 | D_ << dx, dy, dz; 92 | 93 | double attctrl_tau; 94 | nh_private_.param("attctrl_constant", attctrl_tau, 0.1); 95 | nh_private_.param("normalizedthrust_constant", norm_thrust_const_, 0.05); // 1 / max acceleration 96 | nh_private_.param("normalizedthrust_offset", norm_thrust_offset_, 0.1); // 1 / max acceleration 97 | nh_private_.param("Kp_x", Kpos_x_, 8.0); 98 | nh_private_.param("Kp_y", Kpos_y_, 8.0); 99 | nh_private_.param("Kp_z", Kpos_z_, 10.0); 100 | nh_private_.param("Kv_x", Kvel_x_, 1.5); 101 | nh_private_.param("Kv_y", Kvel_y_, 1.5); 102 | nh_private_.param("Kv_z", Kvel_z_, 3.3); 103 | nh_private_.param("posehistory_window", posehistory_window_, 200); 104 | nh_private_.param("init_pos_x", initTargetPos_x_, 0.0); 105 | nh_private_.param("init_pos_y", initTargetPos_y_, 0.0); 106 | nh_private_.param("init_pos_z", initTargetPos_z_, 2.0); 107 | 108 | targetPos_ << initTargetPos_x_, initTargetPos_y_, initTargetPos_z_; // Initial Position 109 | targetVel_ << 0.0, 0.0, 0.0; 110 | mavPos_ << 0.0, 0.0, 0.0; 111 | mavVel_ << 0.0, 0.0, 0.0; 112 | Kpos_ << -Kpos_x_, -Kpos_y_, -Kpos_z_; 113 | Kvel_ << -Kvel_x_, -Kvel_y_, -Kvel_z_; 114 | 115 | bool jerk_enabled = false; 116 | if (!jerk_enabled) { 117 | if (ctrl_mode_ == ERROR_GEOMETRIC) { 118 | controller_ = std::make_shared(attctrl_tau); 119 | } else { 120 | controller_ = std::make_shared(attctrl_tau); 121 | } 122 | } else { 123 | controller_ = std::make_shared(); 124 | } 125 | } 126 | geometricCtrl::~geometricCtrl() { 127 | // Destructor 128 | } 129 | 130 | void geometricCtrl::targetCallback(const geometry_msgs::TwistStamped &msg) { 131 | reference_request_last_ = reference_request_now_; 132 | targetPos_prev_ = targetPos_; 133 | targetVel_prev_ = targetVel_; 134 | 135 | reference_request_now_ = ros::Time::now(); 136 | reference_request_dt_ = (reference_request_now_ - reference_request_last_).toSec(); 137 | 138 | targetPos_ = toEigen(msg.twist.angular); 139 | targetVel_ = toEigen(msg.twist.linear); 140 | 141 | if (reference_request_dt_ > 0) 142 | targetAcc_ = (targetVel_ - targetVel_prev_) / reference_request_dt_; 143 | else 144 | targetAcc_ = Eigen::Vector3d::Zero(); 145 | } 146 | 147 | void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget &msg) { 148 | reference_request_last_ = reference_request_now_; 149 | 150 | targetPos_prev_ = targetPos_; 151 | targetVel_prev_ = targetVel_; 152 | 153 | reference_request_now_ = ros::Time::now(); 154 | reference_request_dt_ = (reference_request_now_ - reference_request_last_).toSec(); 155 | 156 | targetPos_ = toEigen(msg.position); 157 | targetVel_ = toEigen(msg.velocity); 158 | 159 | if (msg.type_mask == 1) { 160 | targetAcc_ = toEigen(msg.acceleration); 161 | targetJerk_ = toEigen(msg.jerk); 162 | targetSnap_ = Eigen::Vector3d::Zero(); 163 | 164 | } else if (msg.type_mask == 2) { 165 | targetAcc_ = toEigen(msg.acceleration); 166 | targetJerk_ = Eigen::Vector3d::Zero(); 167 | targetSnap_ = Eigen::Vector3d::Zero(); 168 | 169 | } else if (msg.type_mask == 4) { 170 | targetAcc_ = Eigen::Vector3d::Zero(); 171 | targetJerk_ = Eigen::Vector3d::Zero(); 172 | targetSnap_ = Eigen::Vector3d::Zero(); 173 | 174 | } else { 175 | targetAcc_ = toEigen(msg.acceleration); 176 | targetJerk_ = toEigen(msg.jerk); 177 | targetSnap_ = toEigen(msg.snap); 178 | } 179 | } 180 | 181 | void geometricCtrl::yawtargetCallback(const std_msgs::Float32 &msg) { 182 | if (!velocity_yaw_) mavYaw_ = double(msg.data); 183 | } 184 | 185 | void geometricCtrl::multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory &msg) { 186 | trajectory_msgs::MultiDOFJointTrajectoryPoint pt = msg.points[0]; 187 | reference_request_last_ = reference_request_now_; 188 | 189 | targetPos_prev_ = targetPos_; 190 | targetVel_prev_ = targetVel_; 191 | 192 | reference_request_now_ = ros::Time::now(); 193 | reference_request_dt_ = (reference_request_now_ - reference_request_last_).toSec(); 194 | 195 | targetPos_ << pt.transforms[0].translation.x, pt.transforms[0].translation.y, pt.transforms[0].translation.z; 196 | targetVel_ << pt.velocities[0].linear.x, pt.velocities[0].linear.y, pt.velocities[0].linear.z; 197 | 198 | targetAcc_ << pt.accelerations[0].linear.x, pt.accelerations[0].linear.y, pt.accelerations[0].linear.z; 199 | targetJerk_ = Eigen::Vector3d::Zero(); 200 | targetSnap_ = Eigen::Vector3d::Zero(); 201 | 202 | if (!velocity_yaw_) { 203 | Eigen::Quaterniond q(pt.transforms[0].rotation.w, pt.transforms[0].rotation.x, pt.transforms[0].rotation.y, 204 | pt.transforms[0].rotation.z); 205 | Eigen::Vector3d rpy = Eigen::Matrix3d(q).eulerAngles(0, 1, 2); // RPY 206 | mavYaw_ = rpy(2); 207 | } 208 | } 209 | 210 | void geometricCtrl::mavposeCallback(const geometry_msgs::PoseStamped &msg) { 211 | if (!received_home_pose) { 212 | received_home_pose = true; 213 | home_pose_ = msg.pose; 214 | ROS_INFO_STREAM("Home pose initialized to: " << home_pose_); 215 | } 216 | mavPos_ = toEigen(msg.pose.position); 217 | mavAtt_(0) = msg.pose.orientation.w; 218 | mavAtt_(1) = msg.pose.orientation.x; 219 | mavAtt_(2) = msg.pose.orientation.y; 220 | mavAtt_(3) = msg.pose.orientation.z; 221 | } 222 | 223 | void geometricCtrl::mavtwistCallback(const geometry_msgs::TwistStamped &msg) { 224 | mavVel_ = toEigen(msg.twist.linear); 225 | mavRate_ = toEigen(msg.twist.angular); 226 | } 227 | 228 | bool geometricCtrl::landCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) { 229 | node_state = LANDING; 230 | return true; 231 | } 232 | 233 | void geometricCtrl::cmdloopCallback(const ros::TimerEvent &event) { 234 | switch (node_state) { 235 | case WAITING_FOR_HOME_POSE: 236 | waitForPredicate(&received_home_pose, "Waiting for home pose..."); 237 | ROS_INFO("Got pose! Drone Ready to be armed."); 238 | node_state = MISSION_EXECUTION; 239 | break; 240 | 241 | case MISSION_EXECUTION: { 242 | Eigen::Vector3d desired_acc; 243 | if (feedthrough_enable_) { 244 | desired_acc = targetAcc_; 245 | } else { 246 | desired_acc = controlPosition(targetPos_, targetVel_, targetAcc_); 247 | } 248 | computeBodyRateCmd(cmdBodyRate_, desired_acc); 249 | pubReferencePose(targetPos_, q_des); 250 | pubRateCommands(cmdBodyRate_, q_des); 251 | appendPoseHistory(); 252 | pubPoseHistory(); 253 | break; 254 | } 255 | 256 | case LANDING: { 257 | geometry_msgs::PoseStamped landingmsg; 258 | landingmsg.header.stamp = ros::Time::now(); 259 | landingmsg.pose = home_pose_; 260 | landingmsg.pose.position.z = landingmsg.pose.position.z + 1.0; 261 | target_pose_pub_.publish(landingmsg); 262 | node_state = LANDED; 263 | ros::spinOnce(); 264 | break; 265 | } 266 | case LANDED: 267 | ROS_INFO("Landed. Please set to position control and disarm."); 268 | cmdloop_timer_.stop(); 269 | break; 270 | } 271 | } 272 | 273 | void geometricCtrl::mavstateCallback(const mavros_msgs::State::ConstPtr &msg) { current_state_ = *msg; } 274 | 275 | void geometricCtrl::statusloopCallback(const ros::TimerEvent &event) { 276 | if (sim_enable_) { 277 | // Enable OFFBoard mode and arm automatically 278 | // This will only run if the vehicle is simulated 279 | mavros_msgs::SetMode offb_set_mode; 280 | arm_cmd_.request.value = true; 281 | offb_set_mode.request.custom_mode = "OFFBOARD"; 282 | if (current_state_.mode != "OFFBOARD" && (ros::Time::now() - last_request_ > ros::Duration(5.0))) { 283 | if (set_mode_client_.call(offb_set_mode) && offb_set_mode.response.mode_sent) { 284 | ROS_INFO("Offboard enabled"); 285 | } 286 | last_request_ = ros::Time::now(); 287 | } else { 288 | if (!current_state_.armed && (ros::Time::now() - last_request_ > ros::Duration(5.0))) { 289 | if (arming_client_.call(arm_cmd_) && arm_cmd_.response.success) { 290 | ROS_INFO("Vehicle armed"); 291 | } 292 | last_request_ = ros::Time::now(); 293 | } 294 | } 295 | } 296 | pubSystemStatus(); 297 | } 298 | 299 | void geometricCtrl::pubReferencePose(const Eigen::Vector3d &target_position, const Eigen::Vector4d &target_attitude) { 300 | geometry_msgs::PoseStamped msg; 301 | 302 | msg.header.stamp = ros::Time::now(); 303 | msg.header.frame_id = "map"; 304 | msg.pose.position.x = target_position(0); 305 | msg.pose.position.y = target_position(1); 306 | msg.pose.position.z = target_position(2); 307 | msg.pose.orientation.w = target_attitude(0); 308 | msg.pose.orientation.x = target_attitude(1); 309 | msg.pose.orientation.y = target_attitude(2); 310 | msg.pose.orientation.z = target_attitude(3); 311 | referencePosePub_.publish(msg); 312 | } 313 | 314 | void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd, const Eigen::Vector4d &target_attitude) { 315 | mavros_msgs::AttitudeTarget msg; 316 | 317 | msg.header.stamp = ros::Time::now(); 318 | msg.header.frame_id = "map"; 319 | msg.body_rate.x = cmd(0); 320 | msg.body_rate.y = cmd(1); 321 | msg.body_rate.z = cmd(2); 322 | msg.type_mask = 128; // Ignore orientation messages 323 | msg.orientation.w = target_attitude(0); 324 | msg.orientation.x = target_attitude(1); 325 | msg.orientation.y = target_attitude(2); 326 | msg.orientation.z = target_attitude(3); 327 | msg.thrust = cmd(3); 328 | 329 | angularVelPub_.publish(msg); 330 | } 331 | 332 | void geometricCtrl::pubPoseHistory() { 333 | nav_msgs::Path msg; 334 | 335 | msg.header.stamp = ros::Time::now(); 336 | msg.header.frame_id = "map"; 337 | msg.poses = posehistory_vector_; 338 | 339 | posehistoryPub_.publish(msg); 340 | } 341 | 342 | void geometricCtrl::pubSystemStatus() { 343 | mavros_msgs::CompanionProcessStatus msg; 344 | 345 | msg.header.stamp = ros::Time::now(); 346 | msg.component = 196; // MAV_COMPONENT_ID_AVOIDANCE 347 | msg.state = (int)companion_state_; 348 | 349 | systemstatusPub_.publish(msg); 350 | } 351 | 352 | void geometricCtrl::appendPoseHistory() { 353 | posehistory_vector_.insert(posehistory_vector_.begin(), vector3d2PoseStampedMsg(mavPos_, mavAtt_)); 354 | if (posehistory_vector_.size() > posehistory_window_) { 355 | posehistory_vector_.pop_back(); 356 | } 357 | } 358 | 359 | geometry_msgs::PoseStamped geometricCtrl::vector3d2PoseStampedMsg(Eigen::Vector3d &position, 360 | Eigen::Vector4d &orientation) { 361 | geometry_msgs::PoseStamped encode_msg; 362 | encode_msg.header.stamp = ros::Time::now(); 363 | encode_msg.header.frame_id = "map"; 364 | encode_msg.pose.orientation.w = orientation(0); 365 | encode_msg.pose.orientation.x = orientation(1); 366 | encode_msg.pose.orientation.y = orientation(2); 367 | encode_msg.pose.orientation.z = orientation(3); 368 | encode_msg.pose.position.x = position(0); 369 | encode_msg.pose.position.y = position(1); 370 | encode_msg.pose.position.z = position(2); 371 | return encode_msg; 372 | } 373 | 374 | Eigen::Vector3d geometricCtrl::controlPosition(const Eigen::Vector3d &target_pos, const Eigen::Vector3d &target_vel, 375 | const Eigen::Vector3d &target_acc) { 376 | /// Compute BodyRate commands using differential flatness 377 | /// Controller based on Faessler 2017 378 | const Eigen::Vector3d a_ref = target_acc; 379 | if (velocity_yaw_) { 380 | mavYaw_ = getVelocityYaw(mavVel_); 381 | } 382 | 383 | const Eigen::Vector4d q_ref = acc2quaternion(a_ref - gravity_, mavYaw_); 384 | const Eigen::Matrix3d R_ref = quat2RotMatrix(q_ref); 385 | 386 | const Eigen::Vector3d pos_error = mavPos_ - target_pos; 387 | const Eigen::Vector3d vel_error = mavVel_ - target_vel; 388 | 389 | // Position Controller 390 | const Eigen::Vector3d a_fb = poscontroller(pos_error, vel_error); 391 | 392 | // Rotor Drag compensation 393 | const Eigen::Vector3d a_rd = R_ref * D_.asDiagonal() * R_ref.transpose() * target_vel; // Rotor drag 394 | 395 | // Reference acceleration 396 | const Eigen::Vector3d a_des = a_fb + a_ref - a_rd - gravity_; 397 | 398 | return a_des; 399 | } 400 | 401 | void geometricCtrl::computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd, const Eigen::Vector3d &a_des) { 402 | // Reference attitude 403 | q_des = acc2quaternion(a_des, mavYaw_); 404 | 405 | controller_->Update(mavAtt_, q_des, a_des, targetJerk_); // Calculate BodyRate 406 | bodyrate_cmd.head(3) = controller_->getDesiredRate(); 407 | double thrust_command = controller_->getDesiredThrust().z(); 408 | bodyrate_cmd(3) = 409 | std::max(0.0, std::min(1.0, norm_thrust_const_ * thrust_command + 410 | norm_thrust_offset_)); // Calculate thrustcontroller_->getDesiredThrust()(3); 411 | } 412 | 413 | Eigen::Vector3d geometricCtrl::poscontroller(const Eigen::Vector3d &pos_error, const Eigen::Vector3d &vel_error) { 414 | Eigen::Vector3d a_fb = 415 | Kpos_.asDiagonal() * pos_error + Kvel_.asDiagonal() * vel_error; // feedforward term for trajectory error 416 | 417 | if (a_fb.norm() > max_fb_acc_) 418 | a_fb = (max_fb_acc_ / a_fb.norm()) * a_fb; // Clip acceleration if reference is too large 419 | 420 | return a_fb; 421 | } 422 | 423 | Eigen::Vector4d geometricCtrl::acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw) { 424 | Eigen::Vector4d quat; 425 | Eigen::Vector3d zb_des, yb_des, xb_des, proj_xb_des; 426 | Eigen::Matrix3d rotmat; 427 | 428 | proj_xb_des << std::cos(yaw), std::sin(yaw), 0.0; 429 | 430 | zb_des = vector_acc / vector_acc.norm(); 431 | yb_des = zb_des.cross(proj_xb_des) / (zb_des.cross(proj_xb_des)).norm(); 432 | xb_des = yb_des.cross(zb_des) / (yb_des.cross(zb_des)).norm(); 433 | 434 | rotmat << xb_des(0), yb_des(0), zb_des(0), xb_des(1), yb_des(1), zb_des(1), xb_des(2), yb_des(2), zb_des(2); 435 | quat = rot2Quaternion(rotmat); 436 | return quat; 437 | } 438 | 439 | bool geometricCtrl::ctrltriggerCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { 440 | unsigned char mode = req.data; 441 | 442 | ctrl_mode_ = mode; 443 | res.success = ctrl_mode_; 444 | res.message = "controller triggered"; 445 | return true; 446 | } 447 | 448 | void geometricCtrl::dynamicReconfigureCallback(geometric_controller::GeometricControllerConfig &config, 449 | uint32_t level) { 450 | if (max_fb_acc_ != config.max_acc) { 451 | max_fb_acc_ = config.max_acc; 452 | ROS_INFO("Reconfigure request : max_acc = %.2f ", config.max_acc); 453 | } else if (Kpos_x_ != config.Kp_x) { 454 | Kpos_x_ = config.Kp_x; 455 | ROS_INFO("Reconfigure request : Kp_x = %.2f ", config.Kp_x); 456 | } else if (Kpos_y_ != config.Kp_y) { 457 | Kpos_y_ = config.Kp_y; 458 | ROS_INFO("Reconfigure request : Kp_y = %.2f ", config.Kp_y); 459 | } else if (Kpos_z_ != config.Kp_z) { 460 | Kpos_z_ = config.Kp_z; 461 | ROS_INFO("Reconfigure request : Kp_z = %.2f ", config.Kp_z); 462 | } else if (Kvel_x_ != config.Kv_x) { 463 | Kvel_x_ = config.Kv_x; 464 | ROS_INFO("Reconfigure request : Kv_x = %.2f ", config.Kv_x); 465 | } else if (Kvel_y_ != config.Kv_y) { 466 | Kvel_y_ = config.Kv_y; 467 | ROS_INFO("Reconfigure request : Kv_y =%.2f ", config.Kv_y); 468 | } else if (Kvel_z_ != config.Kv_z) { 469 | Kvel_z_ = config.Kv_z; 470 | ROS_INFO("Reconfigure request : Kv_z = %.2f ", config.Kv_z); 471 | } 472 | 473 | Kpos_ << -Kpos_x_, -Kpos_y_, -Kpos_z_; 474 | Kvel_ << -Kvel_x_, -Kvel_y_, -Kvel_z_; 475 | } 476 | -------------------------------------------------------------------------------- /geometric_controller/src/geometric_controller_node.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Geometric Controller Node 35 | * 36 | * Geometric controller ROS Node Implementation 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #include "geometric_controller/geometric_controller.h" 42 | 43 | int main(int argc, char** argv) { 44 | ros::init(argc, argv, "geometric_controller"); 45 | ros::NodeHandle nh(""); 46 | ros::NodeHandle nh_private("~"); 47 | 48 | geometricCtrl* geometricController = new geometricCtrl(nh, nh_private); 49 | 50 | dynamic_reconfigure::Server srv; 51 | dynamic_reconfigure::Server::CallbackType f; 52 | f = boost::bind(&geometricCtrl::dynamicReconfigureCallback, geometricController, _1, _2); 53 | srv.setCallback(f); 54 | 55 | ros::spin(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /geometric_controller/src/jerk_tracking_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #include "geometric_controller/jerk_tracking_control.h" 42 | 43 | JerkTrackingControl::JerkTrackingControl() : Control() {} 44 | 45 | JerkTrackingControl::~JerkTrackingControl() {} 46 | 47 | void JerkTrackingControl::Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, 48 | const Eigen::Vector3d &ref_acc, const Eigen::Vector3d &ref_jerk) { 49 | // Jerk feedforward control 50 | // Based on: Lopez, Brett Thomas. Low-latency trajectory planning for high-speed navigation in unknown environments. 51 | // Diss. Massachusetts Institute of Technology, 2016. 52 | // Feedforward control from Lopez(2016) 53 | 54 | double dt_ = 0.01; 55 | // Numerical differentiation to calculate jerk_fb 56 | const Eigen::Vector3d jerk_fb = (ref_acc - last_ref_acc_) / dt_; 57 | const Eigen::Vector3d jerk_des = ref_jerk + jerk_fb; 58 | const Eigen::Matrix3d R = quat2RotMatrix(curr_att); 59 | const Eigen::Vector3d zb = R.col(2); 60 | 61 | const Eigen::Vector3d jerk_vector = 62 | jerk_des / ref_acc.norm() - ref_acc * ref_acc.dot(jerk_des) / std::pow(ref_acc.norm(), 3); 63 | const Eigen::Vector4d jerk_vector4d(0.0, jerk_vector(0), jerk_vector(1), jerk_vector(2)); 64 | 65 | Eigen::Vector4d inverse(1.0, -1.0, -1.0, -1.0); 66 | const Eigen::Vector4d q_inv = inverse.asDiagonal() * curr_att; 67 | const Eigen::Vector4d qd = quatMultiplication(q_inv, ref_att); 68 | 69 | const Eigen::Vector4d qd_star(qd(0), -qd(1), -qd(2), -qd(3)); 70 | 71 | const Eigen::Vector4d ratecmd_pre = quatMultiplication(quatMultiplication(qd_star, jerk_vector4d), qd); 72 | 73 | Eigen::Vector4d ratecmd; 74 | desired_rate_(0) = ratecmd_pre(2); // TODO: Are the coordinate systems consistent? 75 | desired_rate_(1) = (-1.0) * ratecmd_pre(1); 76 | desired_rate_(2) = 0.0; 77 | desired_thrust_(0) = 0.0; 78 | desired_thrust_(1) = 0.0; 79 | desired_thrust_(2) = ref_acc.dot(zb); // Calculate thrust 80 | last_ref_acc_ = ref_acc; 81 | return; 82 | } 83 | -------------------------------------------------------------------------------- /geometric_controller/src/nonlinear_attitude_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #include "geometric_controller/nonlinear_attitude_control.h" 42 | 43 | NonlinearAttitudeControl::NonlinearAttitudeControl(double attctrl_tau) : Control() { attctrl_tau_ = attctrl_tau; } 44 | 45 | NonlinearAttitudeControl::~NonlinearAttitudeControl() {} 46 | 47 | void NonlinearAttitudeControl::Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, 48 | const Eigen::Vector3d &ref_acc, const Eigen::Vector3d &ref_jerk) { 49 | // Geometric attitude controller 50 | // Attitude error is defined as in Brescianini, Dario, Markus Hehn, and Raffaello D'Andrea. Nonlinear quadrocopter 51 | // attitude control: Technical report. ETH Zurich, 2013. 52 | 53 | const Eigen::Vector4d inverse(1.0, -1.0, -1.0, -1.0); 54 | const Eigen::Vector4d q_inv = inverse.asDiagonal() * curr_att; 55 | const Eigen::Vector4d qe = quatMultiplication(q_inv, ref_att); 56 | desired_rate_(0) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(1); 57 | desired_rate_(1) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(2); 58 | desired_rate_(2) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(3); 59 | const Eigen::Matrix3d rotmat = quat2RotMatrix(curr_att); 60 | const Eigen::Vector3d zb = rotmat.col(2); 61 | desired_thrust_(0) = 0.0; 62 | desired_thrust_(1) = 0.0; 63 | desired_thrust_(2) = ref_acc.dot(zb); 64 | } 65 | -------------------------------------------------------------------------------- /geometric_controller/src/nonlinear_geometric_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2022 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Controller base class 36 | * 37 | * 38 | * @author Jaeyoung Lim 39 | */ 40 | 41 | #include "geometric_controller/nonlinear_geometric_control.h" 42 | 43 | NonlinearGeometricControl::NonlinearGeometricControl(double attctrl_tau) : Control() { attctrl_tau_ = attctrl_tau; } 44 | 45 | NonlinearGeometricControl::~NonlinearGeometricControl() {} 46 | 47 | void NonlinearGeometricControl::Update(Eigen::Vector4d &curr_att, const Eigen::Vector4d &ref_att, 48 | const Eigen::Vector3d &ref_acc, const Eigen::Vector3d &ref_jerk) { 49 | // Geometric attitude controller 50 | // Attitude error is defined as in Lee, Taeyoung, Melvin Leok, and N. Harris McClamroch. "Geometric tracking control 51 | // of a quadrotor UAV on SE (3)." 49th IEEE conference on decision and control (CDC). IEEE, 2010. 52 | // The original paper inputs moment commands, but for offboard control, angular rate commands are sent 53 | 54 | Eigen::Vector4d ratecmd; 55 | Eigen::Matrix3d rotmat; // Rotation matrix of current attitude 56 | Eigen::Matrix3d rotmat_d; // Rotation matrix of desired attitude 57 | Eigen::Vector3d error_att; 58 | 59 | rotmat = quat2RotMatrix(curr_att); 60 | rotmat_d = quat2RotMatrix(ref_att); 61 | 62 | error_att = 0.5 * matrix_hat_inv(rotmat_d.transpose() * rotmat - rotmat.transpose() * rotmat_d); 63 | desired_rate_ = (2.0 / attctrl_tau_) * error_att; 64 | const Eigen::Vector3d zb = rotmat.col(2); 65 | desired_thrust_(0) = 0.0; 66 | desired_thrust_(1) = 0.0; 67 | desired_thrust_(2) = ref_acc.dot(zb); 68 | } 69 | -------------------------------------------------------------------------------- /geometric_controller/test/geometric_controller-test.cpp: -------------------------------------------------------------------------------- 1 | #include "geometric_controller/geometric_controller.h" 2 | 3 | #include 4 | #include 5 | 6 | TEST(GeometricControllerTest, acc2quaternion) { 7 | Eigen::Vector3d acceleration; 8 | double yaw; 9 | // Expected result; 10 | Eigen::Vector4d ref_attitude; 11 | Eigen::Vector4d attitude; 12 | 13 | // Condition 14 | acceleration << 0.0, 0.0, 1.0; 15 | yaw = 0.0; 16 | // Expected outcome 17 | ref_attitude << 1.0, 0.0, 0.0, 0.0; 18 | 19 | attitude = geometricCtrl::acc2quaternion(acceleration, yaw); 20 | 21 | ASSERT_TRUE(attitude.isApprox(ref_attitude)); 22 | 23 | // Condition 24 | acceleration << 0.0, 0.0, 1.0; 25 | yaw = 1.5714; 26 | // Expected outcome 27 | ref_attitude << std::cos(0.5 * yaw), 0.0, 0.0, std::sin(0.5 * yaw); 28 | 29 | attitude = geometricCtrl::acc2quaternion(acceleration, yaw); 30 | 31 | ASSERT_TRUE(attitude.isApprox(ref_attitude)); 32 | } 33 | 34 | TEST(GeometricControllerTest, velocityyaw) { 35 | Eigen::Vector3d vel_x(1.0, 0.0, 0.0); 36 | double vel_x_yaw = geometricCtrl::getVelocityYaw(vel_x); 37 | EXPECT_NEAR(0.0, vel_x_yaw, 0.0001); 38 | 39 | Eigen::Vector3d vel_nx(-1.0, 0.0, 0.0); 40 | double vel_nx_yaw = geometricCtrl::getVelocityYaw(vel_nx); 41 | EXPECT_NEAR(M_PI, vel_nx_yaw, 0.0001); 42 | 43 | Eigen::Vector3d vel_y(0.0, 1.0, 0.0); 44 | double vel_y_yaw = geometricCtrl::getVelocityYaw(vel_y); 45 | EXPECT_NEAR(M_PI_2, vel_y_yaw, 0.0001); 46 | 47 | Eigen::Vector3d vel_ny(0.0, -1.0, 0.0); 48 | double vel_ny_yaw = geometricCtrl::getVelocityYaw(vel_ny); 49 | EXPECT_NEAR(-M_PI_2, vel_ny_yaw, 0.0001); 50 | } 51 | -------------------------------------------------------------------------------- /geometric_controller/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) { 4 | ::testing::InitGoogleTest(&argc, argv); 5 | return RUN_ALL_TESTS(); 6 | } 7 | -------------------------------------------------------------------------------- /geometric_controller/test/test_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | using std::string; 5 | 6 | class StringTest : public ::testing::Test { 7 | public: 8 | string actualString; 9 | string wrongString; 10 | string expectString; 11 | 12 | char *actualValTrue; 13 | char *actualValFalse; 14 | char *expectVal; 15 | 16 | // Use this method to set up any state that you need for all of your tests 17 | void SetUp() override { 18 | actualString = "hello gtest"; 19 | wrongString = "hello world"; 20 | expectString = "hello gtest"; 21 | 22 | actualValTrue = new char[actualString.size() + 1]; 23 | strncpy(actualValTrue, actualString.c_str(), actualString.size() + 1); 24 | 25 | actualValFalse = new char[wrongString.size() + 1]; 26 | strncpy(actualValFalse, wrongString.c_str(), wrongString.size() + 1); 27 | 28 | expectVal = new char[expectString.size() + 1]; 29 | strncpy(expectVal, expectString.c_str(), expectString.size() + 1); 30 | } 31 | 32 | // Use this method to clean up any memory, network etc. after each test 33 | void TearDown() override { 34 | delete[] actualValTrue; 35 | delete[] actualValFalse; 36 | delete[] expectVal; 37 | } 38 | }; 39 | 40 | // Example code we are testing: 41 | namespace myNormalCode { 42 | 43 | void reverseInPlace(string &toReverse) { 44 | // NB! this only works for ASCII 45 | for (int i = 0, j = toReverse.size() - 1; i < j; i++, j--) { 46 | char tmp = toReverse[i]; 47 | toReverse[i] = toReverse[j]; 48 | toReverse[j] = tmp; 49 | } 50 | } 51 | } // namespace myNormalCode 52 | 53 | TEST_F(StringTest, StrEqual) { 54 | // GIVEN: two strings that are the same 55 | 56 | // THEN: we expect them to be equal 57 | EXPECT_STREQ(actualString.c_str(), expectString.c_str()); 58 | } 59 | 60 | TEST_F(StringTest, CStrNotEqual) { 61 | // GIVEN: two char* that are NOT the same 62 | 63 | // THEN: we expect them to be not equal 64 | EXPECT_STRNE(expectVal, actualValFalse); 65 | } 66 | 67 | TEST_F(StringTest, testReverse) { 68 | // GIVEN: a string, and a manually reversed string as well 69 | string toReverse = actualString; 70 | const string expectedReversed = "tsetg olleh"; 71 | 72 | // WHEN: we reverse the string 73 | myNormalCode::reverseInPlace(toReverse); 74 | 75 | // THEN: they should be the same 76 | EXPECT_STREQ(expectedReversed.c_str(), toReverse.c_str()); 77 | } -------------------------------------------------------------------------------- /mavros_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mavros_controllers) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /mavros_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mavros_controllers 3 | 0.0.1 4 | Contains useful controllers for mavros 5 | 6 | Jaeyoung Lim 7 | 8 | Jaeyoung Lim 9 | 10 | BSD-3 11 | 12 | https://github.com/Jaeyoung-Lim/mavros_controllers 13 | https://github.com/Jaeyoung-Lim/mavros_controllers/issues 14 | 15 | 16 | catkin 17 | 18 | 19 | controller_msgs 20 | geometric_controller 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /trajectory_publisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(trajectory_publisher) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | tf 10 | mavros 11 | mavros_extras 12 | mavros_msgs 13 | mavlink 14 | controller_msgs 15 | ) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | CATKIN_DEPENDS roscpp rospy std_msgs nav_msgs geometry_msgs tf mav_planning_msgs controller_msgs mavros_msgs 20 | ) 21 | 22 | include_directories( 23 | include 24 | ${Boost_INCLUDE_DIR} 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | ############ 29 | # BINARIES # 30 | ############ 31 | add_executable(trajectory_publisher 32 | src/trajectoryPublisher.cpp 33 | src/trajectoryPublisher_node.cpp 34 | src/trajectory.cpp 35 | src/polynomialtrajectory.cpp 36 | src/shapetrajectory.cpp 37 | ) 38 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 39 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 40 | ########## 41 | # EXPORT # 42 | ########## 43 | #cs_install() 44 | #cs_export() -------------------------------------------------------------------------------- /trajectory_publisher/README.md: -------------------------------------------------------------------------------- 1 | # trajectory_publisher 2 | 3 | ## Overview 4 | Trajectory publisher publishes continous trajectories to the geometric controller 5 | 6 | ## Parameters 7 | - /trajectory_publisher/initpos_x (default: 0.0) 8 | - /trajectory_publisher/initpos_y (default: 0.0) 9 | - /trajectory_publisher/initpos_z (default: 1.0) 10 | - /trajectory_publisher/updaterate (default: 0.01) 11 | - /trajectory_publisher/horizon (default: 1.0) 12 | - /trajectory_publisher/maxjerk (default: 10.0) 13 | - /trajectory_publisher/trajectory_type (default: 0) 14 | - /trajectory_publisher/number_of_primitives (default: 7) 15 | - /trajectory_publisher/shape_radius (default: 1.0) 16 | 17 | 18 | ## Topics 19 | 20 | The geometric controller publishes and subscribes the following topics. 21 | 22 | - Published Topics 23 | - reference/trajectory ( [nav_msgs/Path](http://docs.ros.org/kinetic/api/nav_msgs/html/msg/Path.html) ) 24 | - reference/setpoint ( [geometry_msgs/TwistStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Twist.html) ) 25 | 26 | - Subscribed Topics 27 | - /trajectory_publisher/motionselector ([std_msgs/int32](http://docs.ros.org/api/std_msgs/html/msg/Int32.html)); 28 | - /mavros/local_position/pose ( [geometry_msgs/PoseStamped](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html) ) 29 | - /mavros/local_position/velocity( [geometry_msgs/TwistStamped](http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html) ) -------------------------------------------------------------------------------- /trajectory_publisher/include/trajectory_publisher/polynomialtrajectory.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Polynomial Trajectory 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #ifndef TRAJECTORY_PUBLISHER_POLYNOMIALTRAJECTORY_H 40 | #define TRAJECTORY_PUBLISHER_POLYNOMIALTRAJECTORY_H 41 | 42 | #include "trajectory_publisher/trajectory.h" 43 | 44 | class polynomialtrajectory : public trajectory { 45 | private: 46 | int N; // Degree of polynomial 47 | double dt_; // Sampling time 48 | double T_; 49 | Eigen::Vector4d c_x_, c_y_, c_z_; // Coefficients for polynomial representation 50 | 51 | public: 52 | polynomialtrajectory(); 53 | virtual ~polynomialtrajectory(); 54 | void initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega); 55 | void generatePrimitives(Eigen::Vector3d pos); 56 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel); 57 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d jerk); 58 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc, Eigen::Vector3d jerk); 59 | void setCoefficients(Eigen::VectorXd &x_coefficients, Eigen::VectorXd &y_coefficients, 60 | Eigen::VectorXd &z_coefficients); 61 | Eigen::VectorXd getCoefficients(int dim); 62 | Eigen::Vector3d getPosition(double time); 63 | Eigen::Vector3d getVelocity(double time); 64 | Eigen::Vector3d getAcceleration(double time); 65 | double getsamplingTime() { return dt_; }; 66 | double getDuration() { return T_; }; 67 | nav_msgs::Path getSegment(); 68 | geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d position, Eigen::Vector4d orientation); 69 | }; 70 | 71 | #endif // TRAJECTORY_PUBLISHER_POLYNOMIALTRAJECTORY_H 72 | -------------------------------------------------------------------------------- /trajectory_publisher/include/trajectory_publisher/shapetrajectory.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Shape Trajectory Library 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #ifndef TRAJECTORY_PUBLISHER_SHAPETRAJECTORY_H 40 | #define TRAJECTORY_PUBLISHER_SHAPETRAJECTORY_H 41 | 42 | #include "trajectory_publisher/trajectory.h" 43 | 44 | #define TRAJ_ZERO 0 45 | #define TRAJ_CIRCLE 1 46 | #define TRAJ_LAMNISCATE 2 47 | #define TRAJ_STATIONARY 3 48 | 49 | class shapetrajectory : public trajectory { 50 | private: 51 | int type_; 52 | int N; 53 | double dt_; 54 | double T_; 55 | Eigen::Vector3d traj_axis_; 56 | Eigen::Vector3d traj_origin_; 57 | Eigen::Vector3d traj_radial_; 58 | double traj_radius_, traj_omega_; 59 | 60 | public: 61 | shapetrajectory(int type); 62 | virtual ~shapetrajectory(); 63 | void initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega); 64 | void generatePrimitives(Eigen::Vector3d pos); 65 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel); 66 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d jerk); 67 | void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc, Eigen::Vector3d jerk); 68 | Eigen::Vector3d getPosition(double time); 69 | Eigen::Vector3d getVelocity(double time); 70 | Eigen::Vector3d getAcceleration(double time); 71 | double getsamplingTime() { return dt_; }; 72 | double getDuration() { return T_; }; 73 | nav_msgs::Path getSegment(); 74 | geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d position, Eigen::Vector4d orientation); 75 | }; 76 | #endif // TRAJECTORY_PUBLISHER_SHAPETRAJECTORY_H 77 | -------------------------------------------------------------------------------- /trajectory_publisher/include/trajectory_publisher/trajectory.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Trajectory Base Class 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #ifndef TRAJECTORY_PUBLISHER_TRAJECTORY_H 40 | #define TRAJECTORY_PUBLISHER_TRAJECTORY_H 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | class trajectory { 47 | private: 48 | int N; // Degree of polynomial 49 | double dt_; // Sampling time 50 | double T_; 51 | int type_; 52 | int target_trajectoryID_; 53 | Eigen::Vector4d c_x_, c_y_, c_z_; // Coefficients for polynomial representation 54 | Eigen::Vector3d traj_axis_; 55 | Eigen::Vector3d traj_origin_; 56 | double traj_radius_, traj_omega_; 57 | 58 | public: 59 | trajectory(); 60 | ~trajectory(); 61 | virtual void initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega) = 0; 62 | virtual void generatePrimitives(Eigen::Vector3d pos) = 0; 63 | virtual void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel) = 0; 64 | virtual void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d jerk) = 0; 65 | virtual void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc, 66 | Eigen::Vector3d jerk) = 0; 67 | virtual Eigen::Vector3d getPosition(double time) = 0; 68 | virtual Eigen::Vector3d getVelocity(double time) = 0; 69 | virtual Eigen::Vector3d getAcceleration(double time) = 0; 70 | virtual double getsamplingTime() { return dt_; }; 71 | virtual double getDuration() { return T_; }; 72 | virtual nav_msgs::Path getSegment() = 0; 73 | virtual geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d position, Eigen::Vector4d orientation) = 0; 74 | }; 75 | 76 | #endif // TRAJECTORY_PUBLISHER_TRAJECTORY_H 77 | -------------------------------------------------------------------------------- /trajectory_publisher/include/trajectory_publisher/trajectoryPublisher.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Trajectory Publisher 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #ifndef TRAJECTORYPUBLISHER_H 40 | #define TRAJECTORYPUBLISHER_H 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include "controller_msgs/FlatTarget.h" 60 | #include "trajectory_publisher/polynomialtrajectory.h" 61 | #include "trajectory_publisher/shapetrajectory.h" 62 | #include "trajectory_publisher/trajectory.h" 63 | 64 | #define REF_TWIST 8 65 | #define REF_SETPOINTRAW 16 66 | 67 | using namespace std; 68 | using namespace Eigen; 69 | class trajectoryPublisher { 70 | private: 71 | ros::NodeHandle nh_; 72 | ros::NodeHandle nh_private_; 73 | ros::Publisher trajectoryPub_; 74 | ros::Publisher referencePub_; 75 | ros::Publisher flatreferencePub_; 76 | ros::Publisher rawreferencePub_; 77 | ros::Publisher global_rawreferencePub_; 78 | std::vector primitivePub_; 79 | ros::Subscriber motionselectorSub_; 80 | ros::Subscriber mavposeSub_; 81 | ros::Subscriber mavtwistSub_; 82 | ros::Subscriber mavstate_sub_; 83 | ros::ServiceServer trajtriggerServ_; 84 | ros::Timer trajloop_timer_; 85 | ros::Timer refloop_timer_; 86 | ros::Time start_time_, curr_time_; 87 | 88 | nav_msgs::Path refTrajectory_; 89 | nav_msgs::Path primTrajectory_; 90 | mavros_msgs::State current_state_; 91 | 92 | int trajectory_type_; 93 | Eigen::Vector3d p_targ, v_targ, a_targ; 94 | Eigen::Vector3d p_mav_, v_mav_; 95 | Eigen::Vector3d shape_origin_, shape_axis_; 96 | double shape_omega_ = 0; 97 | double theta_ = 0.0; 98 | double controlUpdate_dt_; 99 | double primitive_duration_; 100 | double trigger_time_; 101 | double init_pos_x_, init_pos_y_, init_pos_z_; 102 | double max_jerk_; 103 | int pubreference_type_; 104 | int num_primitives_; 105 | int motion_selector_; 106 | 107 | std::vector> motionPrimitives_; 108 | std::vector inputs_; 109 | 110 | public: 111 | trajectoryPublisher(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 112 | void updateReference(); 113 | void pubrefTrajectory(int selector); 114 | void pubprimitiveTrajectory(); 115 | void pubrefState(); 116 | void pubflatrefState(); 117 | void pubrefSetpointRaw(); 118 | void pubrefSetpointRawGlobal(); 119 | void initializePrimitives(int type); 120 | void updatePrimitives(); 121 | void loopCallback(const ros::TimerEvent& event); 122 | void refCallback(const ros::TimerEvent& event); 123 | bool triggerCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); 124 | void motionselectorCallback(const std_msgs::Int32& selector); 125 | void mavposeCallback(const geometry_msgs::PoseStamped& msg); 126 | void mavtwistCallback(const geometry_msgs::TwistStamped& msg); 127 | void mavstateCallback(const mavros_msgs::State::ConstPtr& msg); 128 | }; 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /trajectory_publisher/launch/config_file.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Grid1/Status1 11 | - /Pose1 12 | - /Pose2 13 | Splitter Ratio: 0.5 14 | Tree Height: 775 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679016 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Axes Length: 1 57 | Axes Radius: 0.100000001 58 | Class: rviz/Pose 59 | Color: 255; 25; 0 60 | Enabled: true 61 | Head Length: 0.300000012 62 | Head Radius: 0.100000001 63 | Name: Pose 64 | Shaft Length: 1 65 | Shaft Radius: 0.0500000007 66 | Shape: Axes 67 | Topic: /mavros/local_position/pose 68 | Unreliable: false 69 | Value: true 70 | - Alpha: 1 71 | Axes Length: 1 72 | Axes Radius: 0.100000001 73 | Class: rviz/Pose 74 | Color: 0; 255; 0 75 | Enabled: true 76 | Head Length: 0.300000012 77 | Head Radius: 0.100000001 78 | Name: Pose 79 | Shaft Length: 1 80 | Shaft Radius: 0.0500000007 81 | Shape: Axes 82 | Topic: /iris/reference/pose 83 | Unreliable: false 84 | Value: true 85 | Enabled: true 86 | Global Options: 87 | Background Color: 48; 48; 48 88 | Default Light: true 89 | Fixed Frame: map 90 | Frame Rate: 30 91 | Name: root 92 | Tools: 93 | - Class: rviz/Interact 94 | Hide Inactive Objects: true 95 | - Class: rviz/MoveCamera 96 | - Class: rviz/Select 97 | - Class: rviz/FocusCamera 98 | - Class: rviz/Measure 99 | - Class: rviz/SetInitialPose 100 | Topic: /initialpose 101 | - Class: rviz/SetGoal 102 | Topic: /move_base_simple/goal 103 | - Class: rviz/PublishPoint 104 | Single click: true 105 | Topic: /clicked_point 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz/Orbit 110 | Distance: 10 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.0599999987 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 0 118 | Y: 0 119 | Z: 0 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.0500000007 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.00999999978 125 | Pitch: 0.465398014 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 0.735397995 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: false 133 | Height: 1056 134 | Hide Left Dock: false 135 | Hide Right Dock: false 136 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 137 | Selection: 138 | collapsed: false 139 | Time: 140 | collapsed: false 141 | Tool Properties: 142 | collapsed: false 143 | Views: 144 | collapsed: false 145 | Width: 1920 146 | X: 0 147 | Y: 24 148 | -------------------------------------------------------------------------------- /trajectory_publisher/launch/mav_trajectory_setpointraw.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 | -------------------------------------------------------------------------------- /trajectory_publisher/launch/sitl_trajectory_setpointraw.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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /trajectory_publisher/launch/trajectory_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /trajectory_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trajectory_publisher 4 | 0.0.0 5 | The trajectory_publisher package 6 | 7 | 8 | 9 | 10 | Jaeyoung Lim 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | catkin 19 | roscpp 20 | rospy 21 | std_msgs 22 | nav_msgs 23 | geometry_msgs 24 | tf 25 | mav_planning_msgs 26 | controller_msgs 27 | mavros_msgs 28 | roscpp 29 | rospy 30 | std_msgs 31 | geometry_msgs 32 | nav_msgs 33 | tf 34 | mav_planning_msgs 35 | mavros_msgs 36 | controller_msgs 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /trajectory_publisher/src/polynomialtrajectory.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Polynomial Trajectory 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #include "trajectory_publisher/polynomialtrajectory.h" 40 | 41 | polynomialtrajectory::polynomialtrajectory() : N(0), dt_(0.1), T_(1.0) { 42 | c_x_ << 0.0, 0.0, 0.0, 0.0; 43 | c_y_ << 0.0, 0.0, 0.0, 0.0; 44 | c_z_ << 0.0, 0.0, 0.0, 0.0; 45 | }; 46 | 47 | polynomialtrajectory::~polynomialtrajectory() {} 48 | 49 | void polynomialtrajectory::setCoefficients(Eigen::VectorXd &x_coefficients, Eigen::VectorXd &y_coefficients, 50 | Eigen::VectorXd &z_coefficients) { 51 | c_x_ = x_coefficients; 52 | c_y_ = y_coefficients; 53 | c_z_ = z_coefficients; 54 | } 55 | 56 | void polynomialtrajectory::initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega) { 57 | // Generate primitives based on current state for smooth trajectory 58 | c_x_(0) = pos(0); 59 | c_y_(0) = pos(1); 60 | c_z_(0) = pos(2); 61 | } 62 | 63 | void polynomialtrajectory::generatePrimitives(Eigen::Vector3d pos) { 64 | // Generate primitives based on current state for smooth trajectory 65 | c_x_(0) = pos(0); 66 | c_y_(0) = pos(1); 67 | c_z_(0) = pos(2); 68 | } 69 | 70 | void polynomialtrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel) { 71 | // Generate primitives based on current state for smooth trajectory 72 | c_x_(0) = pos(0); 73 | c_y_(0) = pos(1); 74 | c_z_(0) = pos(2); 75 | 76 | c_x_(1) = vel(0); 77 | c_y_(1) = vel(1); 78 | c_z_(1) = vel(2); 79 | } 80 | 81 | void polynomialtrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d jerk) { 82 | // Generate primitives based on current state for smooth trajectory 83 | c_x_(0) = pos(0); 84 | c_y_(0) = pos(1); 85 | c_z_(0) = pos(2); 86 | 87 | c_x_(1) = vel(0); 88 | c_y_(1) = vel(1); 89 | c_z_(1) = vel(2); 90 | 91 | c_x_(2) = 0.0; // Acceleration is neglected 92 | c_y_(2) = 0.0; 93 | c_z_(2) = 0.0; 94 | 95 | c_x_(3) = jerk(0); 96 | c_y_(3) = jerk(1); 97 | c_z_(3) = jerk(2); 98 | } 99 | 100 | void polynomialtrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc, 101 | Eigen::Vector3d jerk) { 102 | // Generate primitives based on current state for smooth trajectory 103 | c_x_(0) = pos(0); 104 | c_y_(0) = pos(1); 105 | c_z_(0) = pos(2); 106 | 107 | c_x_(1) = vel(0); 108 | c_y_(1) = vel(1); 109 | c_z_(1) = vel(2); 110 | 111 | c_x_(2) = acc(0); 112 | c_y_(2) = acc(1); 113 | c_z_(2) = acc(2); 114 | 115 | c_x_(3) = jerk(0); 116 | c_y_(3) = jerk(1); 117 | c_z_(3) = jerk(2); 118 | } 119 | 120 | Eigen::VectorXd polynomialtrajectory::getCoefficients(int dim) { 121 | switch (dim) { 122 | case 0: 123 | return c_x_; 124 | case 1: 125 | return c_y_; 126 | case 2: 127 | return c_z_; 128 | } 129 | } 130 | 131 | Eigen::Vector3d polynomialtrajectory::getPosition(double time) { 132 | Eigen::Vector3d position; 133 | position << c_x_(0) + c_x_(1) * time + c_x_(2) * pow(time, 2) + c_x_(3) * pow(time, 3), 134 | c_y_(0) + c_y_(1) * time + c_y_(2) * pow(time, 2) + c_y_(3) * pow(time, 3), 135 | c_z_(0) + c_z_(1) * time + c_z_(2) * pow(time, 2) + c_z_(3) * pow(time, 3); 136 | 137 | return position; 138 | } 139 | 140 | Eigen::Vector3d polynomialtrajectory::getVelocity(double time) { 141 | Eigen::Vector3d velocity; 142 | velocity << c_x_(1) + c_x_(2) * time * 2 + c_x_(3) * pow(time, 2) * 3, 143 | c_y_(1) + c_y_(2) * time * 2 + c_y_(3) * pow(time, 2) * 3, 144 | c_z_(1) + c_z_(2) * time * 2 + c_z_(3) * pow(time, 2) * 3; 145 | 146 | return velocity; 147 | } 148 | 149 | Eigen::Vector3d polynomialtrajectory::getAcceleration(double time) { 150 | Eigen::Vector3d acceleration; 151 | acceleration << c_x_(2) * 2 + c_x_(3) * time * 6, c_y_(2) * 2 + c_y_(3) * time * 6, c_z_(2) * 2 + c_z_(3) * time * 6; 152 | 153 | return acceleration; 154 | } 155 | 156 | nav_msgs::Path polynomialtrajectory::getSegment() { 157 | Eigen::Vector3d targetPosition; 158 | Eigen::Vector4d targetOrientation; 159 | nav_msgs::Path segment; 160 | 161 | targetOrientation << 1.0, 0.0, 0.0, 0.0; 162 | geometry_msgs::PoseStamped targetPoseStamped; 163 | 164 | for (double t = 0; t < this->getDuration(); t += this->getsamplingTime()) { 165 | targetPosition = this->getPosition(t); 166 | targetPoseStamped = vector3d2PoseStampedMsg(targetPosition, targetOrientation); 167 | segment.poses.push_back(targetPoseStamped); 168 | } 169 | return segment; 170 | } 171 | 172 | geometry_msgs::PoseStamped polynomialtrajectory::vector3d2PoseStampedMsg(Eigen::Vector3d position, 173 | Eigen::Vector4d orientation) { 174 | geometry_msgs::PoseStamped encode_msg; 175 | encode_msg.header.stamp = ros::Time::now(); 176 | encode_msg.header.frame_id = "map"; 177 | encode_msg.pose.orientation.w = orientation(0); 178 | encode_msg.pose.orientation.x = orientation(1); 179 | encode_msg.pose.orientation.y = orientation(2); 180 | encode_msg.pose.orientation.z = orientation(3); 181 | encode_msg.pose.position.x = position(0); 182 | encode_msg.pose.position.y = position(1); 183 | encode_msg.pose.position.z = position(2); 184 | 185 | return encode_msg; 186 | } -------------------------------------------------------------------------------- /trajectory_publisher/src/shapetrajectory.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Shape Trajectory Library 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #include "trajectory_publisher/shapetrajectory.h" 40 | 41 | shapetrajectory::shapetrajectory(int type) : trajectory(), N(0), dt_(0.1), T_(10.0), type_(type) { 42 | traj_omega_ = 2.0; 43 | traj_axis_ << 0.0, 0.0, 1.0; 44 | traj_radial_ << 1.0, 0.0, 0.0; 45 | traj_origin_ << 0.0, 0.0, 1.0; 46 | }; 47 | 48 | shapetrajectory::~shapetrajectory(){ 49 | 50 | }; 51 | 52 | void shapetrajectory::initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega) { 53 | // Generate primitives based on current state for smooth trajectory 54 | traj_origin_ = pos; 55 | traj_omega_ = omega; 56 | T_ = 2 * 3.14 / traj_omega_; 57 | traj_axis_ = axis; 58 | traj_radial_ << 2.0, 0.0, 0.0; 59 | } 60 | 61 | void shapetrajectory::generatePrimitives(Eigen::Vector3d pos) {} 62 | 63 | void shapetrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel) {} 64 | 65 | void shapetrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d jerk) {} 66 | 67 | void shapetrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc, 68 | Eigen::Vector3d jerk) {} 69 | 70 | Eigen::Vector3d shapetrajectory::getPosition(double time) { 71 | Eigen::Vector3d position; 72 | double theta; 73 | 74 | switch (type_) { 75 | case TRAJ_ZERO: 76 | 77 | position << 0.0, 0.0, 0.0; 78 | break; 79 | 80 | case TRAJ_CIRCLE: 81 | 82 | theta = traj_omega_ * time; 83 | position = std::cos(theta) * traj_radial_ + std::sin(theta) * traj_axis_.cross(traj_radial_) + 84 | (1 - std::cos(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_ + traj_origin_; 85 | break; 86 | 87 | case TRAJ_LAMNISCATE: // Lemniscate of Genero 88 | 89 | theta = traj_omega_ * time; 90 | position = std::cos(theta) * traj_radial_ + std::sin(theta) * std::cos(theta) * traj_axis_.cross(traj_radial_) + 91 | (1 - std::cos(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_ + traj_origin_; 92 | break; 93 | case TRAJ_STATIONARY: // Lemniscate of Genero 94 | 95 | position = traj_origin_; 96 | break; 97 | } 98 | return position; 99 | } 100 | 101 | Eigen::Vector3d shapetrajectory::getVelocity(double time) { 102 | Eigen::Vector3d velocity; 103 | double theta; 104 | 105 | switch (type_) { 106 | case TRAJ_CIRCLE: 107 | 108 | velocity = traj_omega_ * traj_axis_.cross(getPosition(time)); 109 | break; 110 | case TRAJ_STATIONARY: 111 | 112 | velocity << 0.0, 0.0, 0.0; 113 | break; 114 | 115 | case TRAJ_LAMNISCATE: // Lemniscate of Genero 116 | 117 | theta = traj_omega_ * time; 118 | velocity = traj_omega_ * 119 | (-std::sin(theta) * traj_radial_ + 120 | (std::pow(std::cos(theta), 2) - std::pow(std::sin(theta), 2)) * traj_axis_.cross(traj_radial_) + 121 | (std::sin(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_); 122 | break; 123 | 124 | default: 125 | velocity << 0.0, 0.0, 0.0; 126 | break; 127 | } 128 | return velocity; 129 | } 130 | 131 | Eigen::Vector3d shapetrajectory::getAcceleration(double time) { 132 | Eigen::Vector3d acceleration; 133 | 134 | switch (type_) { 135 | case TRAJ_CIRCLE: 136 | 137 | acceleration = traj_omega_ * traj_axis_.cross(getVelocity(time)); 138 | break; 139 | case TRAJ_STATIONARY: 140 | 141 | acceleration << 0.0, 0.0, 0.0; 142 | break; 143 | default: 144 | acceleration << 0.0, 0.0, 0.0; 145 | break; 146 | } 147 | return acceleration; 148 | } 149 | 150 | nav_msgs::Path shapetrajectory::getSegment() { 151 | Eigen::Vector3d targetPosition; 152 | Eigen::Vector4d targetOrientation; 153 | nav_msgs::Path segment; 154 | 155 | targetOrientation << 1.0, 0.0, 0.0, 0.0; 156 | geometry_msgs::PoseStamped targetPoseStamped; 157 | 158 | for (double t = 0; t < this->getDuration(); t += this->getsamplingTime()) { 159 | targetPosition = this->getPosition(t); 160 | targetPoseStamped = vector3d2PoseStampedMsg(targetPosition, targetOrientation); 161 | segment.poses.push_back(targetPoseStamped); 162 | } 163 | return segment; 164 | } 165 | 166 | geometry_msgs::PoseStamped shapetrajectory::vector3d2PoseStampedMsg(Eigen::Vector3d position, 167 | Eigen::Vector4d orientation) { 168 | geometry_msgs::PoseStamped encode_msg; 169 | encode_msg.header.stamp = ros::Time::now(); 170 | encode_msg.header.frame_id = "map"; 171 | encode_msg.pose.orientation.w = orientation(0); 172 | encode_msg.pose.orientation.x = orientation(1); 173 | encode_msg.pose.orientation.y = orientation(2); 174 | encode_msg.pose.orientation.z = orientation(3); 175 | encode_msg.pose.position.x = position(0); 176 | encode_msg.pose.position.y = position(1); 177 | encode_msg.pose.position.z = position(2); 178 | return encode_msg; 179 | } 180 | -------------------------------------------------------------------------------- /trajectory_publisher/src/trajectory.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Trajectory Base Class 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #include "trajectory_publisher/trajectory.h" 40 | 41 | trajectory::trajectory(){ 42 | 43 | }; 44 | 45 | trajectory::~trajectory(){ 46 | 47 | }; -------------------------------------------------------------------------------- /trajectory_publisher/src/trajectoryPublisher.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Trajectory Publisher 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #include "trajectory_publisher/trajectoryPublisher.h" 40 | 41 | using namespace std; 42 | using namespace Eigen; 43 | trajectoryPublisher::trajectoryPublisher(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) 44 | : nh_(nh), nh_private_(nh_private), motion_selector_(0) { 45 | trajectoryPub_ = nh_.advertise("trajectory_publisher/trajectory", 1); 46 | referencePub_ = nh_.advertise("reference/setpoint", 1); 47 | flatreferencePub_ = nh_.advertise("reference/flatsetpoint", 1); 48 | rawreferencePub_ = nh_.advertise("mavros/setpoint_raw/local", 1); 49 | global_rawreferencePub_ = nh_.advertise("mavros/setpoint_raw/global", 1); 50 | motionselectorSub_ = 51 | nh_.subscribe("trajectory_publisher/motionselector", 1, &trajectoryPublisher::motionselectorCallback, this, 52 | ros::TransportHints().tcpNoDelay()); 53 | mavposeSub_ = nh_.subscribe("mavros/local_position/pose", 1, &trajectoryPublisher::mavposeCallback, this, 54 | ros::TransportHints().tcpNoDelay()); 55 | mavtwistSub_ = nh_.subscribe("mavros/local_position/velocity", 1, &trajectoryPublisher::mavtwistCallback, this, 56 | ros::TransportHints().tcpNoDelay()); 57 | mavstate_sub_ = nh_.subscribe("mavros/state", 1, &trajectoryPublisher::mavstateCallback, this, 58 | ros::TransportHints().tcpNoDelay()); 59 | 60 | trajloop_timer_ = nh_.createTimer(ros::Duration(0.1), &trajectoryPublisher::loopCallback, this); 61 | refloop_timer_ = nh_.createTimer(ros::Duration(0.01), &trajectoryPublisher::refCallback, this); 62 | 63 | trajtriggerServ_ = nh_.advertiseService("start", &trajectoryPublisher::triggerCallback, this); 64 | 65 | nh_private_.param("initpos_x", init_pos_x_, 0.0); 66 | nh_private_.param("initpos_y", init_pos_y_, 0.0); 67 | nh_private_.param("initpos_z", init_pos_z_, 1.0); 68 | nh_private_.param("updaterate", controlUpdate_dt_, 0.01); 69 | nh_private_.param("horizon", primitive_duration_, 1.0); 70 | nh_private_.param("maxjerk", max_jerk_, 10.0); 71 | nh_private_.param("shape_omega", shape_omega_, 1.5); 72 | nh_private_.param("trajectory_type", trajectory_type_, 0); 73 | nh_private_.param("number_of_primitives", num_primitives_, 7); 74 | nh_private_.param("reference_type", pubreference_type_, 2); 75 | 76 | inputs_.resize(num_primitives_); 77 | 78 | if (trajectory_type_ == 0) { // Polynomial Trajectory 79 | 80 | if (num_primitives_ == 7) { 81 | inputs_.at(0) << 0.0, 0.0, 0.0; // Constant jerk inputs for minimim time trajectories 82 | inputs_.at(1) << 1.0, 0.0, 0.0; 83 | inputs_.at(2) << -1.0, 0.0, 0.0; 84 | inputs_.at(3) << 0.0, 1.0, 0.0; 85 | inputs_.at(4) << 0.0, -1.0, 0.0; 86 | inputs_.at(5) << 0.0, 0.0, 1.0; 87 | inputs_.at(6) << 0.0, 0.0, -1.0; 88 | } 89 | 90 | for (int i = 0; i < num_primitives_; i++) { 91 | motionPrimitives_.emplace_back(std::make_shared()); 92 | primitivePub_.push_back( 93 | nh_.advertise("trajectory_publisher/primitiveset" + std::to_string(i), 1)); 94 | inputs_.at(i) = inputs_.at(i) * max_jerk_; 95 | } 96 | } else { // Shape trajectories 97 | 98 | num_primitives_ = 1; 99 | motionPrimitives_.emplace_back(std::make_shared(trajectory_type_)); 100 | primitivePub_.push_back(nh_.advertise("trajectory_publisher/primitiveset", 1)); 101 | } 102 | 103 | p_targ << init_pos_x_, init_pos_y_, init_pos_z_; 104 | v_targ << 0.0, 0.0, 0.0; 105 | shape_origin_ << init_pos_x_, init_pos_y_, init_pos_z_; 106 | shape_axis_ << 0.0, 0.0, 1.0; 107 | motion_selector_ = 0; 108 | 109 | initializePrimitives(trajectory_type_); 110 | } 111 | 112 | void trajectoryPublisher::updateReference() { 113 | curr_time_ = ros::Time::now(); 114 | if (current_state_.mode != "OFFBOARD") { /// Reset start_time_ when not in offboard 115 | start_time_ = ros::Time::now(); 116 | } 117 | trigger_time_ = (curr_time_ - start_time_).toSec(); 118 | 119 | p_targ = motionPrimitives_.at(motion_selector_)->getPosition(trigger_time_); 120 | v_targ = motionPrimitives_.at(motion_selector_)->getVelocity(trigger_time_); 121 | if (pubreference_type_ != 0) a_targ = motionPrimitives_.at(motion_selector_)->getAcceleration(trigger_time_); 122 | } 123 | 124 | void trajectoryPublisher::initializePrimitives(int type) { 125 | if (type == 0) { 126 | for (int i = 0; i < motionPrimitives_.size(); i++) 127 | motionPrimitives_.at(i)->generatePrimitives(p_mav_, v_mav_, inputs_.at(i)); 128 | } else { 129 | for (int i = 0; i < motionPrimitives_.size(); i++) 130 | motionPrimitives_.at(i)->initPrimitives(shape_origin_, shape_axis_, shape_omega_); 131 | // TODO: Pass in parameters for primitive trajectories 132 | } 133 | } 134 | 135 | void trajectoryPublisher::updatePrimitives() { 136 | for (int i = 0; i < motionPrimitives_.size(); i++) motionPrimitives_.at(i)->generatePrimitives(p_mav_, v_mav_); 137 | } 138 | 139 | void trajectoryPublisher::pubrefTrajectory(int selector) { 140 | // Publish current trajectory the publisher is publishing 141 | refTrajectory_ = motionPrimitives_.at(selector)->getSegment(); 142 | refTrajectory_.header.stamp = ros::Time::now(); 143 | refTrajectory_.header.frame_id = "map"; 144 | trajectoryPub_.publish(refTrajectory_); 145 | } 146 | 147 | void trajectoryPublisher::pubprimitiveTrajectory() { 148 | for (int i = 0; i < motionPrimitives_.size(); i++) { 149 | primTrajectory_ = motionPrimitives_.at(i)->getSegment(); 150 | primTrajectory_.header.stamp = ros::Time::now(); 151 | primTrajectory_.header.frame_id = "map"; 152 | primitivePub_.at(i).publish(primTrajectory_); 153 | } 154 | } 155 | 156 | void trajectoryPublisher::pubrefState() { 157 | geometry_msgs::TwistStamped msg; 158 | 159 | msg.header.stamp = ros::Time::now(); 160 | msg.header.frame_id = "map"; 161 | msg.twist.angular.x = p_targ(0); 162 | msg.twist.angular.y = p_targ(1); 163 | msg.twist.angular.z = p_targ(2); 164 | msg.twist.linear.x = v_targ(0); 165 | msg.twist.linear.y = v_targ(1); 166 | msg.twist.linear.z = v_targ(2); 167 | referencePub_.publish(msg); 168 | } 169 | 170 | void trajectoryPublisher::pubflatrefState() { 171 | controller_msgs::FlatTarget msg; 172 | 173 | msg.header.stamp = ros::Time::now(); 174 | msg.header.frame_id = "map"; 175 | msg.type_mask = pubreference_type_; 176 | msg.position.x = p_targ(0); 177 | msg.position.y = p_targ(1); 178 | msg.position.z = p_targ(2); 179 | msg.velocity.x = v_targ(0); 180 | msg.velocity.y = v_targ(1); 181 | msg.velocity.z = v_targ(2); 182 | msg.acceleration.x = a_targ(0); 183 | msg.acceleration.y = a_targ(1); 184 | msg.acceleration.z = a_targ(2); 185 | flatreferencePub_.publish(msg); 186 | } 187 | 188 | void trajectoryPublisher::pubrefSetpointRaw() { 189 | mavros_msgs::PositionTarget msg; 190 | msg.header.stamp = ros::Time::now(); 191 | msg.header.frame_id = "map"; 192 | msg.type_mask = 0; 193 | msg.position.x = p_targ(0); 194 | msg.position.y = p_targ(1); 195 | msg.position.z = p_targ(2); 196 | msg.velocity.x = v_targ(0); 197 | msg.velocity.y = v_targ(1); 198 | msg.velocity.z = v_targ(2); 199 | msg.acceleration_or_force.x = a_targ(0); 200 | msg.acceleration_or_force.y = a_targ(1); 201 | msg.acceleration_or_force.z = a_targ(2); 202 | rawreferencePub_.publish(msg); 203 | } 204 | 205 | void trajectoryPublisher::pubrefSetpointRawGlobal() { 206 | mavros_msgs::GlobalPositionTarget msg; 207 | 208 | msg.header.stamp = ros::Time::now(); 209 | msg.header.frame_id = "map"; 210 | msg.type_mask = 0; 211 | msg.coordinate_frame = 5; 212 | msg.latitude = 47.397742; 213 | msg.longitude = 8.545594; 214 | msg.altitude = 500.0; 215 | msg.velocity.x = v_targ(0); 216 | msg.velocity.y = v_targ(1); 217 | msg.velocity.z = v_targ(2); 218 | msg.acceleration_or_force.x = a_targ(0); 219 | msg.acceleration_or_force.y = a_targ(1); 220 | msg.acceleration_or_force.z = a_targ(2); 221 | global_rawreferencePub_.publish(msg); 222 | } 223 | 224 | void trajectoryPublisher::mavstateCallback(const mavros_msgs::State::ConstPtr& msg) { current_state_ = *msg; } 225 | 226 | void trajectoryPublisher::loopCallback(const ros::TimerEvent& event) { 227 | // Slow Loop publishing trajectory information 228 | pubrefTrajectory(motion_selector_); 229 | pubprimitiveTrajectory(); 230 | } 231 | 232 | void trajectoryPublisher::refCallback(const ros::TimerEvent& event) { 233 | // Fast Loop publishing reference states 234 | updateReference(); 235 | switch (pubreference_type_) { 236 | case REF_TWIST: 237 | pubrefState(); 238 | break; 239 | case REF_SETPOINTRAW: 240 | pubrefSetpointRaw(); 241 | // pubrefSetpointRawGlobal(); 242 | break; 243 | default: 244 | pubflatrefState(); 245 | break; 246 | } 247 | } 248 | 249 | bool trajectoryPublisher::triggerCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { 250 | unsigned char mode = req.data; 251 | 252 | start_time_ = ros::Time::now(); 253 | res.success = true; 254 | res.message = "trajectory triggered"; 255 | } 256 | 257 | void trajectoryPublisher::motionselectorCallback(const std_msgs::Int32& selector_msg) { 258 | motion_selector_ = selector_msg.data; 259 | updatePrimitives(); 260 | start_time_ = ros::Time::now(); 261 | } 262 | 263 | void trajectoryPublisher::mavposeCallback(const geometry_msgs::PoseStamped& msg) { 264 | p_mav_(0) = msg.pose.position.x; 265 | p_mav_(1) = msg.pose.position.y; 266 | p_mav_(2) = msg.pose.position.z; 267 | updatePrimitives(); 268 | } 269 | 270 | void trajectoryPublisher::mavtwistCallback(const geometry_msgs::TwistStamped& msg) { 271 | v_mav_(0) = msg.twist.linear.x; 272 | v_mav_(1) = msg.twist.linear.y; 273 | v_mav_(2) = msg.twist.linear.z; 274 | updatePrimitives(); 275 | } -------------------------------------------------------------------------------- /trajectory_publisher/src/trajectoryPublisher_node.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2021 Jaeyoung Lim. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | /** 34 | * @brief Trajectory Publisher ROS Node 35 | * 36 | * @author Jaeyoung Lim 37 | */ 38 | 39 | #include "trajectory_publisher/trajectoryPublisher.h" 40 | 41 | int main(int argc, char **argv) { 42 | ros::init(argc, argv, "trajectory_publisher"); 43 | ros::NodeHandle nh(""); 44 | ros::NodeHandle nh_private("~"); 45 | trajectoryPublisher referencePublisher(nh, nh_private); 46 | ros::spin(); 47 | return 0; 48 | } 49 | --------------------------------------------------------------------------------