├── .clang-format ├── .cmake-format.yaml ├── .github └── workflows │ ├── build_and_test.yml │ └── cpplint.yml ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── doc ├── Lissajous.md ├── MultiGazebo.md ├── MultiMatlab.md ├── MultiSim.md ├── PythonExample.md ├── QuadGazebo.md ├── QuadSim.md ├── kr_mav_control_block_diag.png └── kr_mav_control_block_diagram.pptx ├── interfaces ├── kr_crazyflie_interface │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ ├── cf_gains.yaml │ │ └── crazyflie.yaml │ ├── launch │ │ └── test.launch │ ├── nodelet_plugin.xml │ ├── package.xml │ ├── src │ │ └── so3cmd_to_crazyflie_nodelet.cpp │ └── thrust_fit_matlab │ │ └── thrust_fitting.m ├── kr_matlab_interface │ ├── NewOdomEventData.m │ ├── QuadControlRos.m │ ├── example_interface.m │ ├── kr_mav_manager.patch.package.xml │ └── kr_mav_msgs.patch.xml ├── kr_mavros_interface │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ └── test.launch │ ├── nodelet_plugin.xml │ ├── package.xml │ └── src │ │ └── so3cmd_to_mavros_nodelet.cpp ├── kr_python_interface │ ├── CMakeLists.txt │ ├── package.xml │ ├── setup.py │ └── src │ │ └── kr_python_interface │ │ ├── __init__.py │ │ ├── mav_example.py │ │ └── mav_interface.py ├── kr_qualcomm_interface │ ├── CMakeLists.txt │ ├── cmake │ │ └── FindSnav.cmake │ ├── config │ │ ├── gains.yaml │ │ ├── mav_manager_params.yaml │ │ ├── tracker_params.yaml │ │ └── trackers.yaml │ ├── launch │ │ ├── snav_interface.launch │ │ └── snav_publisher.launch │ ├── nodelet_plugin.xml │ ├── package.xml │ └── src │ │ ├── poscmd_to_snav_nodelet.cpp │ │ ├── snav_publisher.cpp │ │ ├── so3cmd_to_qualcomm_nodelet.cpp │ │ ├── trpycmd_to_snav_nodelet.cpp │ │ └── vio_odom_publisher.cpp ├── kr_rosflight_interface │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ └── test.launch │ ├── nodelet_plugin.xml │ ├── package.xml │ └── src │ │ └── so3cmd_to_rosflight_nodelet.cpp └── kr_serial_interface │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include │ └── kr_serial_interface │ │ ├── ASIOSerialDevice.h │ │ ├── comm_types.h │ │ ├── decode_msgs.h │ │ ├── encode_msgs.h │ │ └── serial_interface.h │ ├── launch │ └── test.launch │ ├── nodelet_plugin.xml │ ├── package.xml │ └── src │ ├── ASIOSerialDevice.cc │ ├── asctec_serial_interface.cpp │ ├── decode_msgs.cpp │ ├── encode_msgs.cpp │ ├── quad_decode_msg_nodelet.cpp │ ├── quad_encode_msg.cpp │ ├── quad_encode_msg_nodelet.cpp │ └── quad_serial_comm_nodelet.cpp ├── kr_mav_controllers ├── CMakeLists.txt ├── cfg │ └── SO3.cfg ├── include │ └── kr_mav_controllers │ │ ├── PIDControl.hpp │ │ ├── SO3Control.h │ │ └── so3_control_tester.hpp ├── nodelet_plugin.xml ├── package.xml ├── src │ ├── PIDControl.cpp │ ├── SO3Control.cpp │ ├── pid_control_nodelet.cpp │ ├── so3_control_nodelet.cpp │ └── so3_trpy_control.cpp └── test │ ├── so3_control_nodelet.test │ └── so3_control_nodelet_test.cpp ├── kr_mav_launch ├── CMakeLists.txt ├── config │ ├── gains.yaml │ ├── mav_manager_params.yaml │ ├── tracker_params.yaml │ └── trackers.yaml ├── launch │ ├── asctec_comms.launch │ ├── demo.launch │ ├── example_control.launch │ ├── mesh_vis.launch │ ├── rviz.launch │ ├── rviz_config.rviz │ └── sim.launch ├── package.xml └── scripts │ ├── demo_lissajous.py │ ├── sample.bash │ └── takeoff.sh ├── kr_mav_manager ├── CMakeLists.txt ├── README.md ├── include │ └── kr_mav_manager │ │ ├── manager.h │ │ └── mav_manager_services.h ├── package.xml ├── src │ ├── manager.cpp │ ├── mav_services.cpp │ └── sample_sub.cpp └── srv │ ├── Circle.srv │ ├── CompoundLissajous.srv │ ├── GoalTimed.srv │ ├── Lissajous.srv │ └── Vec4.srv ├── kr_mav_msgs ├── CMakeLists.txt ├── msg │ ├── AuxCommand.msg │ ├── Corrections.msg │ ├── MotorRPM.msg │ ├── OutputData.msg │ ├── PWMCommand.msg │ ├── PositionCommand.msg │ ├── SO3Command.msg │ ├── Serial.msg │ ├── StatusData.msg │ └── TRPYCommand.msg └── package.xml ├── kr_mesh_visualization ├── CMakeLists.txt ├── launch │ └── test.launch ├── mesh │ ├── hummingbird.mesh │ └── nano.mesh ├── package.xml └── src │ └── mesh_visualization.cpp ├── kr_quadrotor_simulator ├── CMakeLists.txt ├── config │ ├── crazyflie_params.yaml │ ├── hummingbird_params.yaml │ └── qualcomm_quad_params.yaml ├── include │ └── kr_quadrotor_simulator │ │ └── Quadrotor.h ├── package.xml └── src │ ├── dynamics │ └── Quadrotor.cpp │ ├── quadrotor_simulator_base.hpp │ ├── quadrotor_simulator_so3.cpp │ └── quadrotor_simulator_trpy.cpp ├── rqt_mav_manager ├── CMakeLists.txt ├── package.xml ├── plugin.xml ├── resource │ └── MavManager.ui ├── scripts │ └── rqt_mav_manager ├── setup.py └── src │ └── rqt_mav_manager │ └── __init__.py └── trackers ├── kr_tracker_msgs ├── CMakeLists.txt ├── action │ ├── CircleTracker.action │ ├── LineTracker.action │ ├── LissajousAdder.action │ ├── LissajousTracker.action │ ├── TrajectoryTracker.action │ └── VelocityTracker.action ├── msg │ ├── TrackerStatus.msg │ └── VelocityGoal.msg ├── package.xml └── srv │ └── Transition.srv ├── kr_trackers ├── CMakeLists.txt ├── include │ └── kr_trackers │ │ ├── initial_conditions.h │ │ ├── lissajous_generator.h │ │ └── traj_gen.h ├── matlab │ └── line_tracker_min_jerk_numerical_issues.m ├── nodelet_plugin.xml ├── package.xml ├── scripts │ ├── twist_to_velocity_goal.py │ └── waypoints_to_action.py └── src │ ├── circle_tracker_server.cpp │ ├── initial_conditions.cpp │ ├── line_tracker_distance_server.cpp │ ├── line_tracker_min_jerk_server.cpp │ ├── line_tracker_trapezoid.cpp │ ├── line_tracker_yaw.cpp │ ├── lissajous_adder_server.cpp │ ├── lissajous_generator.cpp │ ├── lissajous_tracker_server.cpp │ ├── null_tracker.cpp │ ├── smooth_vel_tracker_server.cpp │ ├── traj_gen.cpp │ ├── trajectory_tracker.cpp │ └── velocity_tracker.cpp └── kr_trackers_manager ├── CMakeLists.txt ├── include └── kr_trackers_manager │ └── Tracker.h ├── nodelet_plugin.xml ├── package.xml └── src └── trackers_manager.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AllowShortFunctionsOnASingleLine: Inline 5 | AllowShortIfStatementsOnASingleLine: false 6 | AllowShortLoopsOnASingleLine: false 7 | AlwaysBreakBeforeMultilineStrings: false 8 | BreakBeforeBraces: Allman 9 | BreakBeforeTernaryOperators: false 10 | ColumnLimit: 120 11 | DerivePointerAlignment: false 12 | PointerAlignment: Right 13 | SpaceBeforeParens: Never 14 | ... 15 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | additional_commands: 2 | foo: 3 | flags: 4 | - BAR 5 | - BAZ 6 | kwargs: 7 | DEPENDS: "*" 8 | HEADERS: "*" 9 | SOURCES: "*" 10 | bullet_char: "*" 11 | dangle_parens: false 12 | enum_char: . 13 | line_ending: unix 14 | line_width: 120 15 | max_pargs_hwrap: 4 16 | separate_ctrl_name_with_space: false 17 | separate_fn_name_with_space: false 18 | tab_size: 2 19 | -------------------------------------------------------------------------------- /.github/workflows/build_and_test.yml: -------------------------------------------------------------------------------- 1 | name: build-and-test 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | build: 11 | strategy: 12 | matrix: 13 | ros_distro: [melodic, noetic] 14 | 15 | runs-on: ubuntu-latest 16 | container: osrf/ros:${{ matrix.ros_distro }}-desktop 17 | steps: 18 | - uses: actions/checkout@v2 19 | 20 | - name: Install dependencies 21 | run: | 22 | apt-get update 23 | apt-get install -qy g++ libeigen3-dev python3-catkin-tools 24 | rosdep update 25 | rosdep install --from-paths . --ignore-src -y -r --as-root apt:false 26 | 27 | - name: Setup catkin workspace 28 | run: | 29 | . /opt/ros/${{ matrix.ros_distro }}/setup.sh 30 | mkdir -p ${RUNNER_WORKSPACE}/catkin_ws/src 31 | cd ${RUNNER_WORKSPACE}/catkin_ws/src 32 | ln -s ${GITHUB_WORKSPACE} 33 | catkin_init_workspace . 34 | 35 | - name: Build workspace and run tests 36 | run: | 37 | . /opt/ros/${{ matrix.ros_distro }}/setup.sh 38 | cd ${RUNNER_WORKSPACE}/catkin_ws 39 | export LDFLAGS="-Wl,-O1,--sort-common,--as-needed,--no-undefined,-z,relro,-z,now -pthread" 40 | catkin build -j4 --no-status -DCMAKE_BUILD_TYPE=Release 41 | . ${RUNNER_WORKSPACE}/catkin_ws/devel/setup.sh 42 | catkin test 43 | -------------------------------------------------------------------------------- /.github/workflows/cpplint.yml: -------------------------------------------------------------------------------- 1 | name: Reviewdog 2 | on: [pull_request] 3 | 4 | jobs: 5 | cpplint: 6 | runs-on: ubuntu-latest 7 | steps: 8 | - uses: actions/checkout@master 9 | - uses: reviewdog/action-cpplint@master 10 | with: 11 | github_token: ${{ secrets.github_token }} 12 | reporter: github-pr-review 13 | flags: --linelength=120 14 | filter: "-readability/braces\ 15 | ,-whitespace/braces\ 16 | ,-whitespace/parens\ 17 | ,-whitespace/newline\ 18 | ,-build/c++11" 19 | # -readability/braces\ 20 | # ,-whitespace/comments\ 21 | # ,-whitespace/indent\ 22 | # ,-whitespace/operators\ 23 | # " # Optional 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ROS 2 | devel/ 3 | logs/ 4 | build/ 5 | bin/ 6 | lib/ 7 | msg_gen/ 8 | srv_gen/ 9 | msg/*Action.msg 10 | msg/*ActionFeedback.msg 11 | msg/*ActionGoal.msg 12 | msg/*ActionResult.msg 13 | msg/*Feedback.msg 14 | msg/*Goal.msg 15 | msg/*Result.msg 16 | msg/_*.py 17 | build_isolated/ 18 | devel_isolated/ 19 | 20 | # Generated by dynamic reconfigure 21 | *.cfgc 22 | /cfg/cpp/ 23 | /cfg/*.py 24 | 25 | # Ignore generated docs 26 | *.dox 27 | *.wikidoc 28 | 29 | # eclipse stuff 30 | .project 31 | .cproject 32 | 33 | # qcreator stuff 34 | CMakeLists.txt.user 35 | 36 | srv/_*.py 37 | *.pcd 38 | *.pyc 39 | qtcreator-* 40 | *.user 41 | 42 | /planning/cfg 43 | /planning/docs 44 | /planning/src 45 | 46 | *~ 47 | 48 | # Emacs 49 | .#* 50 | 51 | # Catkin custom files 52 | CATKIN_IGNORE 53 | 54 | *.bag 55 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/.gitmodules -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, KumarRobotics at University of Pennsylvania 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | kr_mav_control 2 | ================= 3 | 4 | ROS packages for quadrotor control 5 | 6 | [![Build Status](https://github.com/KumarRobotics/kr_mav_control/workflows/build-and-test/badge.svg)](https://github.com/KumarRobotics/kr_mav_control/actions?query=workflow%3Abuild) 7 | 8 | ### Stack includes: 9 | 10 | - `kr_mav_manager`: A manager for the kr_mav_control stack 11 | - `rqt_mav_manager`: GUI interface for common kr_mav_manager functions 12 | - `interfaces`: Translates `kr_mav_msgs/SO3Command` to corresponding robots interface. 13 | - `kr_quadrotor_simulator`: Simple quadrotor dynamics simulator 14 | - `kr_mav_msgs`: Common msgs used across packages 15 | - `kr_mav_controllers`: Position controllers 16 | - `trackers`: Different trackers under `kr_trackers`, and `kr_trackers_manager` 17 | 18 | ### Example use cases: 19 | 20 | The multi robot examples uses following packages. 21 | 22 | * [kr_multi_mav_manager](https://github.com/KumarRobotics/multi_mav_manager) 23 | * [mrsl_quadrotor](https://github.com/KumarRobotics/mrsl_quadrotor) 24 | * [waypoint_navigation_tool](https://github.com/KumarRobotics/waypoint_navigation_plugin) 25 | 26 | [Running single robot with the included simple simulator](doc/QuadSim.md) 27 | 28 | [Running multiple robots with the included simple simulator](doc/MultiSim.md) 29 | 30 | [MATLAB interface with simple simulator](doc/MultiMatlab.md) 31 | 32 | [Python interface with simple simulator](doc/PythonExample.md) 33 | 34 | [Running with Gazebo](doc/QuadGazebo.md) 35 | 36 | [Running multiple robots with Gazebo](doc/MultiGazebo.md) 37 | 38 | This code has been tested with ROS Noetic on Ubuntu 20.04. 39 | 40 | ### Block Diagram 41 | 42 | The following block diagram shows how the packages in the repo fit together. 43 | ![Block Diagram](doc/kr_mav_control_block_diag.png) 44 | 45 | Further detailed breakdown of the blocks can be found in 46 | the [PPT](doc/kr_mav_control_block_diagram.pptx) 47 | -------------------------------------------------------------------------------- /doc/Lissajous.md: -------------------------------------------------------------------------------- 1 | # The Simple and Compound Lissajous Trackers 2 | 3 | This document shows how to use the simple 4 | and compound lissajous trackers. These trackers are used to create 5 | 3D Lissajous trajectories and sums of such trajectories. 6 | 7 | ## Mathematical Trajectory Description and Parameters 8 | 9 | The general form of the 3D Lissajous trace, parameterized 10 | by the variable `s`, is given as follows: 11 | 12 | ``` 13 | x(s) = x_amp * (1 - cos(2 * PI * x_num_periods * s / period)) 14 | y(s) = y_amp * sin(2 * PI * y_num_periods * s / period) 15 | z(s) = z_amp * sin(2 * PI * z_num_periods * s / period) 16 | yaw(s) = yaw_amp * sin(2 * PI * yaw_num_periods * s / period) 17 | ``` 18 | 19 | The trajectory is shaped by shaping the `x_amp`, `y_amp`, `z_amp`, `yaw_amp`, 20 | `x_num_periods`, `y_num_periods`, `z_num_periods`, `yaw_num_periods`, and 21 | `period` parameters. 22 | 23 | Since such a trajectory does not ramp up and ramp down smoothly, the 24 | trajectory is reparameterized by a smooth ramp-up and ramp-down function 25 | given by an 8th order polynomial. The ramp-up function is paramterized by 26 | the ramp time `ramp_time`. 27 | 28 | Finally, te trajectory may be repeated. This is the function of the 29 | `num_cycles` parameter. 30 | 31 | ## Commanding a Lissajous 32 | 33 | The Lissajous tracker and adder may be called by the approprate MAV services 34 | (lissajous and compound, respectively). For example, a normal Lissajous: 35 | 36 | ``` 37 | rosservice call /quadrotor_name/mav_services/lissajous "{x_amp: 1.25, y_amp: 1.25, z_amp: 0.75, yaw_amp: 3.1415, x_num_periods: 12, y_num_periods: 12, z_num_periods: 16, yaw_num_periods: 5, period: 60, num_cycles: 1, ramp_time: 2}" 38 | ``` 39 | 40 | ``` 41 | rosservice call /dragonfly1/mav_services/compound_lissajous "x_amp: [1.25, 0.1] 42 | y_amp: [1.25, 0.1] 43 | z_amp: [0.75, 0.3] 44 | yaw_amp: [3.1415, 0.5] 45 | x_num_periods: [12, 5.0] 46 | y_num_periods: [12, 5.0] 47 | z_num_periods: [16, 1.0] 48 | yaw_num_periods: [5.0, 3.0] 49 | period: [60.0, 60.0] 50 | num_cycles: [1.0, 1.0] 51 | ramp_time: [2.0, 2.0]" 52 | ``` 53 | 54 | A demo with 2 MAVs: 55 | 56 | Dependency: `sudo apt install ros-$ROS_DISTRO-smach-ros` 57 | ``` 58 | roscd kr_multi_mav_manager/scripts 59 | ./demo_sim 2 60 | ``` 61 | 62 | Motors ON -> Take Off 63 | 64 | ``` 65 | roscd kr_mav_launch/scripts 66 | ./demo_lissajous.py 67 | ``` -------------------------------------------------------------------------------- /doc/MultiGazebo.md: -------------------------------------------------------------------------------- 1 | ## Example with multiple simulated robots 2 | 3 | Clone and build [kr_multi_mav_manager](https://github.com/KumarRobotics/multi_mav_manager) in your workspace 4 | 5 | Helper bash scripts are added to launch multiple robots. (Requires tmux installed) 6 | ``` 7 | roscd kr_multi_mav_manager/scripts 8 | ./demo_gazebo.sh 2 9 | ``` 10 | * This will launch 2 robots in gazebo. Do not run more if your machine cannot support. 11 | * Switch to the `Kill` tab in the `tmux` window and press `Enter` to close everything 12 | * `dragonfly$ID` namespace is used for each robot. Each vehicle can be interfaced with the kr_mav_control topics/services in this namespace. 13 | * Use the rqt_multi_mav_gui GUI to control all the robots. Wait until you see `==== Multi MAV Manager is ready for action ===` in one of the tmux pane. 14 | * You can also use the MATLAB interface to control the robots. Might have to update the `odom_topic` in the interface. 15 | 16 | If MAVs need to be loaded in pre-defined start locations, create a CSV file to input X,Y,Z,YAW on each new line per MAV. An example csv is provided. 17 | ``` 18 | roscd kr_multi_mav_manager/scripts 19 | ./demo_gazebo.sh start_locations.csv 20 | ``` 21 | -------------------------------------------------------------------------------- /doc/MultiMatlab.md: -------------------------------------------------------------------------------- 1 | ## Example with multiple simulated robots and MATLAB (Compatible with 2018b and later versions) 2 | 3 | MATLAB wrappers for `kr_mav_control` using the [Robotics Systems Toolbox](https://www.mathworks.com/help/robotics/index.html?s_tid=CRUX_lftnav) are available in `kr_matlab_interface`. 4 | 5 | Make sure you have the toolbox installed. In addition, for the interface to work with `kr_mav_control`, ROS custom messages have to be generated. 6 | 7 | In MATLAB console 8 | ``` 9 | roboticsAddons 10 | ``` 11 | * Install 12 | * `Robotics System Toolbox Interface for ROS Custom Messages` 13 | * `Robotics System Toolbox UAV Library` (Only available for 2018b and later versions) 14 | 15 | Once installed follow the following instructions to generate custom messages used in `kr_mav_control`. More info on generating ROS custom messages is [here](https://www.mathworks.com/help/robotics/ref/rosgenmsg.html) 16 | 17 | ``` 18 | cd ~/ws_ros/src/kr_mav_control 19 | mkdir -p ~/matlab_msgs 20 | cp -r kr_mav_msgs kr_mav_manager ~/matlab_msgs 21 | cd ~/ws_ros/src/kr_mav_control/interfaces/kr_matlab_interface 22 | cp kr_mav_msgs.patch.xml ~/matlab_msgs/kr_mav_msgs/package.xml 23 | cp kr_mav_manager.patch.package.xml ~/matlab_msgs/kr_mav_manager/package.xml 24 | cd ~/matlab_msgs 25 | git clone https://github.com/ros/ros_comm_msgs.git 26 | cd ros_comm_msgs 27 | mv std_srvs .. 28 | ``` 29 | * Newer format 2 of `package.xml` has issues with MATLAB message generation. Hence, an older patched version is used. 30 | * MATLABs inbuild `std_srvs` does not have all the needed services. Hence, we clone and compile it again locally. 31 | 32 | Open MATLAB and run the following 33 | 34 | ``` 35 | rosgenmsg('~/matlab_msgs') 36 | ``` 37 | 38 | * Follow the instructions spilled out in MATLAB console, i.e edit `javaclasspath.txt` and `addpath` with necessary locations. 39 | 40 | Clone and build [kr_multi_mav_manager](https://github.com/KumarRobotics/multi_mav_manager) in your workspace 41 | 42 | ## Running simple simulator with kr_matlab_interface 43 | 44 | Helper bash scripts are added to launch multiple robots. 45 | ``` 46 | roscd kr_multi_mav_manager/scripts 47 | ./demo_sim.sh 4 48 | ``` 49 | * This will launch 4 robots in simulator 50 | * Switch to the `Kill` tab in the `tmux` window and press `Enter` to close everything 51 | * `dragonfly$ID` namespace is used for each robot. Each vehicle can be interfaced with the kr_mav_control topics/services in this namespace. 52 | 53 | In MATLAB 54 | ``` 55 | cd ~/ws_ros/src/kr_mav_control/interfaces/kr_matlab_interface 56 | example_interface('localhost', 4) 57 | ``` 58 | * This starts the motors, calls takeoff and moves all the 4 MAVs with random velocity. 59 | * The first string is the hostname. Use `localhost` if the simulators are running on your own machine. 60 | * Copy and reuse the `example_interface.m` to develop/test your own awesome MATLAB based algorithms. 61 | * This is a minimal interface. More API specific to kr_mav_control can be added easily. 62 | * If due to some issue, MATLAB gets stuck in a loop, type `rosshutdown` at the console. This will shutdown all subscribers. 63 | 64 | Happy `MATLABing` 65 | -------------------------------------------------------------------------------- /doc/MultiSim.md: -------------------------------------------------------------------------------- 1 | ## Example with multiple simulated robots 2 | 3 | Clone and build [kr_multi_mav_manager](https://github.com/KumarRobotics/multi_mav_manager) in your workspace 4 | 5 | Helper bash scripts are added to launch multiple robots. (Requires tmux installed) 6 | ``` 7 | roscd kr_multi_mav_manager/scripts 8 | ./demo_sim.sh 4 9 | ``` 10 | * This will launch 4 robots in simulator 11 | * Switch to the `Kill` tab in the `tmux` window and press `Enter` to close everything 12 | * `dragonfly$ID` namespace is used for each robot. Each vehicle can be interfaced with the kr_mav_control topics/services in this namespace. 13 | * Use the rqt_kr_multi_mav_manager GUI to control all the robots. Wait until you see `==== Multi MAV Manager is ready for action ===` in one of the tmux pane. 14 | 15 | If MAVs need to be loaded in pre-defined start locations, create a CSV file to input X,Y,Z,YAW on each new line per MAV. An example csv is provided. 16 | ``` 17 | roscd kr_multi_mav_manager/scripts 18 | ./demo_sim.sh start_locations.csv 19 | ``` 20 | -------------------------------------------------------------------------------- /doc/PythonExample.md: -------------------------------------------------------------------------------- 1 | ## Example with a simulated robot and Python 2 | 3 | ## Running simple simulator with kr_python_interface 4 | 5 | ``` 6 | roscd kr_multi_mav_manager/scripts 7 | ./demo_sim.sh 1 8 | ``` 9 | In another terminal 10 | ``` 11 | rosrun kr_python_interface mav_example.py 12 | ``` 13 | * Switch to the `Kill` tab in the `tmux` window and press `Enter` to close everything 14 | * `dragonfly$ID` namespace is used for each robot. Each vehicle can be interfaced with the kr_mav_control topics/services in this namespace. -------------------------------------------------------------------------------- /doc/QuadGazebo.md: -------------------------------------------------------------------------------- 1 | Various packages exists to simulate a quadrotor in gazebo. [RotorS](https://github.com/ethz-asl/rotors_simulator), [PX4SITL](https://github.com/PX4/sitl_gazebo), [Hector](https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor), etc are some of the few. 2 | 3 | We use our own simplified gazebo plugins/simulator (https://github.com/KumarRobotics/mrsl_quadrotor) 4 | 5 | 6 | ``` 7 | roslaunch mrsl_quadrotor_launch gazebo.launch world:=levine 8 | roslaunch mrsl_quadrotor_launch spawn.launch mav_type:=pelican mav_name:=juliett 9 | ``` 10 | 11 | This spawns a quadrotor with plugins for `odom` and `so3_cmd` under `juliett` namespace. kr_mav_control plugins have to be launched under this namespace. 12 | 13 | ``` 14 | roslaunch mrsl_quadrotor_launch controller.launch odom_topic:=ground_truth/odom mav_type:=pelican mav_name:=juliett mass:=0.5 15 | 16 | ``` 17 | 18 | Launch the GUI to control the robot. Change robot name to `juliett` 19 | ``` 20 | rosrun rqt_mav_manager rqt_mav_manager 21 | ``` -------------------------------------------------------------------------------- /doc/QuadSim.md: -------------------------------------------------------------------------------- 1 | ## Example with GUI 2 | ``` 3 | roslaunch kr_mav_launch rviz.launch 4 | roslaunch kr_mav_launch demo.launch sim:=true vicon:=false mav_name:=quadrotor 5 | ``` 6 | 7 | Run a simple example script 8 | ``` 9 | roscd kr_mav_launch/scripts 10 | ./sample.bash quadrotor 11 | ``` 12 | 13 | There is also a GUI that can be used to send simple commands to the robot through the `rqt_mav_manager`. Launch it by running 14 | ``` 15 | rosrun rqt_mav_manager rqt_mav_manager 16 | ``` 17 | then try Motors ON -> Take Off -> Go To (set z > 0) 18 | 19 | ## Example with Waypoint Navigation Tool 20 | 21 | Clone the [waypoint_navigation_tool](https://github.com/KumarRobotics/waypoint_navigation_plugin) in your workspace. 22 | 23 | ``` 24 | roslaunch kr_mav_launch rviz.launch 25 | roslaunch kr_mav_launch demo.launch sim:=true vicon:=false mav_name:=quadrotor 26 | rosrun kr_trackers waypoints_to_action.py __ns:=quadrotor 27 | ``` 28 | 29 | Use rqt to start motors and takeoff. 30 | ``` 31 | rosrun rqt_mav_manager rqt_mav_manager 32 | ``` 33 | * then try Motors ON -> Take Off -> Go To (set z > 0) 34 | 35 | Use rviz to place waypoints and publish on the topic `/waypoints`. The `waypoints_to_action` node listens to this topic, sends an action goal to `TrajectoryTracker` and switches the tracker. 36 | -------------------------------------------------------------------------------- /doc/kr_mav_control_block_diag.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/doc/kr_mav_control_block_diag.png -------------------------------------------------------------------------------- /doc/kr_mav_control_block_diagram.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/doc/kr_mav_control_block_diagram.pptx -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_crazyflie_interface) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS roscpp 17 | nav_msgs 18 | geometry_msgs 19 | kr_mav_msgs 20 | nodelet) 21 | find_package(Eigen3 REQUIRED) 22 | 23 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) 24 | 25 | catkin_package( 26 | LIBRARIES 27 | ${PROJECT_NAME} 28 | CATKIN_DEPENDS 29 | roscpp 30 | nav_msgs 31 | geometry_msgs 32 | kr_mav_msgs 33 | nodelet 34 | DEPENDS 35 | EIGEN3) 36 | 37 | add_library(${PROJECT_NAME} src/so3cmd_to_crazyflie_nodelet.cpp) 38 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 39 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 40 | 41 | install( 42 | TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 46 | 47 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 48 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 49 | -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/README.md: -------------------------------------------------------------------------------- 1 | ## Crazyflie_interface 2 | 3 | This node translates kr_mav_msgs/SO3Command to geometry_msgs/Twist. 4 | 5 | Note: To fly a crazyflie, you will also need: 6 | 7 | [crazyflie_ros](https://github.com/whoenig/crazyflie_ros) -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/config/cf_gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 8.000, y: 8.000, z: 13.000} 3 | vel: {x: 5.000, y: 5.000, z: 5.000} 4 | ki: {x: 0.1, y: 0.1, z: 0.2} 5 | kib: {x: 0.1, y: 0.1, z: 0.1} 6 | 7 | max_pos_int: 2.6 8 | max_pos_int_b: 2.6 9 | max_tilt_angle: 3.14 -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/config/crazyflie.yaml: -------------------------------------------------------------------------------- 1 | kp_yaw_rate: 0.1 2 | c1: -0.6709 3 | c2: 0.1932 4 | c3: 13.0652 5 | so3_cmd_timeout: 0.1 6 | thrust_pwm_max: 60000 7 | thrust_pwm_min: 20000 -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This reformats an so3_command and publishes it on compatabile Crazyflie topics 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_crazyflie_interface 3 | 1.0.0 4 | kr_crazyflie_interface 5 | Justin Thomas 6 | Aaron Weinstein 7 | 8 | BSD 9 | 10 | 11 | catkin 12 | 13 | 14 | roscpp 15 | nav_msgs 16 | geometry_msgs 17 | kr_mav_msgs 18 | nodelet 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /interfaces/kr_crazyflie_interface/thrust_fit_matlab/thrust_fitting.m: -------------------------------------------------------------------------------- 1 | %% Bitcraze experiment used pwm (0-256) as the input and measured force 2 | 3 | pwm_bc = 1:256; 4 | 5 | a_bc = 0.409e-3; 6 | b_bc = 140.5e-3; 7 | c_bc = -0.999; 8 | 9 | F_bc = 0.409e-3 * pwm_bc.^2 + 140.5e-3 * pwm_bc - 0.999; 10 | 11 | %% Non dimensionalize the pwm input to (0-1) 12 | a_nd = a_bc * 256^2; 13 | b_nd = b_bc * 256; 14 | c_nd = c_bc; 15 | pwm_nd = 0:.01:1; 16 | 17 | F_nd = a_nd * pwm_nd.^2 + b_nd * pwm_nd + c_nd; 18 | 19 | %% Use quadratic formula to invert the non dimensional result 20 | % The goal here is to calculate the pwm input to create a desired thrust 21 | 22 | % Hard code these as parameters for the crazyflies 23 | c1 = -b_nd / (2*a_nd); 24 | c2 = 1/sqrt(a_nd); 25 | c3 = (b_nd^2/(4*a_nd)) - c_nd; 26 | 27 | thrust=0:70; 28 | power_in = c1 + c2 * sqrt(c3 + thrust); 29 | 30 | hold on; 31 | axis([0 70 0 1.1]); 32 | plot(F_bc, (pwm_bc/256), 'b'); 33 | plot(thrust, power_in, 'mo'); 34 | -------------------------------------------------------------------------------- /interfaces/kr_matlab_interface/NewOdomEventData.m: -------------------------------------------------------------------------------- 1 | classdef (ConstructOnLoad) NewOdomEventData < event.EventData 2 | properties 3 | agent_number 4 | position 5 | orientation 6 | end 7 | 8 | methods 9 | function data = NewOdomEventData(agent_number, position, orientation) 10 | data.agent_number = agent_number; 11 | data.position = position; 12 | data.orientation = orientation; 13 | end 14 | end 15 | end 16 | 17 | -------------------------------------------------------------------------------- /interfaces/kr_matlab_interface/QuadControlRos.m: -------------------------------------------------------------------------------- 1 | classdef QuadControlRos < handle 2 | properties (SetAccess = private) 3 | n_agents = [] 4 | agent = [] 5 | agent_ids = [] 6 | agent_namespace = [] 7 | end 8 | 9 | events 10 | NewOdom 11 | end 12 | 13 | methods 14 | function [obj] = QuadControlRos(hostname, n_agents, agent_namespace) 15 | if nargin < 2 16 | error('Need hostname and n_agents as argument') 17 | return; 18 | end 19 | 20 | if n_agents < 1 21 | obj.n_agents = 1; 22 | else 23 | obj.n_agents = n_agents; 24 | end 25 | 26 | if(~isstr(hostname)) 27 | disp('hostname has to be a string, setting to localhost') 28 | hostname = 'localhost'; 29 | end 30 | 31 | if(~isstr(agent_namespace)) 32 | disp('agent_namespace has to be string, using dragonfly') 33 | obj.agent_namespace = 'dragonfly'; 34 | else 35 | obj.agent_namespace = agent_namespace; 36 | end 37 | 38 | obj.agent_ids = 1:n_agents; %TODO add ability to pass non sequential agent ids 39 | rosinit(hostname) 40 | 41 | pause(0.5) 42 | disp('rosinit successful, setting up subscribers, publishers and service clients'); 43 | obj.setup_subs(); 44 | disp('ready'); 45 | end 46 | 47 | function setup_subs(obj) 48 | 49 | for n_ag = 1:obj.n_agents 50 | goto_topic = sprintf('/%s%d/mav_services/goTo',obj.agent_namespace, obj.agent_ids(n_ag)); 51 | obj.agent(n_ag).goto_srv = rossvcclient(goto_topic, 'Timeout', 10); 52 | 53 | %TODO use kr_multi_mav_manager interface? Will reduce number of 54 | %subscribers in MATLAB 55 | motors_topic = sprintf('/%s%d/mav_services/motors',obj.agent_namespace, obj.agent_ids(n_ag)); 56 | obj.agent(n_ag).motors_srv = rossvcclient(motors_topic, 'Timeout', 10); 57 | 58 | takeoff_topic = sprintf('/%s%d/mav_services/takeoff',obj.agent_namespace, obj.agent_ids(n_ag)); 59 | obj.agent(n_ag).takeoff_srv = rossvcclient(takeoff_topic, 'Timeout', 10); 60 | 61 | twist_topic = sprintf('/%s%d/cmd_vel',obj.agent_namespace, obj.agent_ids(n_ag)); 62 | obj.agent(n_ag).twist_pub = rospublisher(twist_topic,'geometry_msgs/Twist'); 63 | end 64 | 65 | %Setup callbacks at the end 66 | for n_ag = 1:obj.n_agents 67 | odom_topic = sprintf('/%s%d/odom',obj.agent_namespace, obj.agent_ids(n_ag)); 68 | obj.agent(n_ag).odom_sub = rossubscriber(odom_topic, {@obj.odom_sub_cb, n_ag}); 69 | end 70 | 71 | disp('Starting sub/pub'); 72 | pause(1) 73 | end 74 | 75 | function delete(obj) 76 | % obj is always scalar 77 | for n_ag = 1:obj.n_agents 78 | delete(obj.agent(n_ag).odom_sub); 79 | end 80 | rosshutdown; 81 | end 82 | 83 | function[] = send_twist(obj,agent_number, twist) 84 | twistdata = rosmessage('geometry_msgs/Twist'); 85 | twistdata.Linear.X = twist(1); 86 | twistdata.Linear.Y = twist(2); 87 | twistdata.Linear.Z = twist(3); 88 | 89 | twistdata.Angular.Z = twist(4); 90 | 91 | send(obj.agent(agent_number).twist_pub,twistdata); 92 | end 93 | 94 | function[] = send_zero_twist(obj, agent_number) 95 | twistdata = rosmessage('geometry_msgs/Twist'); 96 | twistdata.Linear.X = 0; 97 | twistdata.Linear.Y = 0; 98 | twistdata.Linear.Z = 0; 99 | 100 | twistdata.Angular.Z = 0; 101 | 102 | send(obj.agent(agent_number).twist_pub,twistdata); 103 | end 104 | 105 | function[response] = send_wp(obj, agent_number, wp) 106 | request = rosmessage(obj.agent(agent_number).goto_srv); 107 | request.Goal = [wp(1), wp(2), wp(3), wp(4)]; 108 | response = call(obj.agent(agent_number).goto_srv, request); 109 | end 110 | 111 | function[response] = motors(obj, agent_number, motors) 112 | request = rosmessage(obj.agent(agent_number).motors_srv); 113 | request.Data = motors; 114 | response = call(obj.agent(agent_number).motors_srv, request); 115 | end 116 | 117 | function[response] = takeoff(obj, agent_number) 118 | request = rosmessage(obj.agent(agent_number).takeoff_srv); 119 | response = call(obj.agent(agent_number).takeoff_srv, request); 120 | end 121 | 122 | function [odom] = getOdom(obj,agent_number) 123 | odom = obj.agent(agent_number).odom; 124 | end 125 | 126 | function[] = odom_sub_cb(obj, ~, msg, agent_number) 127 | obj.agent(agent_number).odom = msg; 128 | 129 | pt = msg.Pose.Pose.Position; 130 | qt = msg.Pose.Pose.Orientation; 131 | position = [pt.X, pt.Y, pt.Z]; 132 | orientation = [qt.W, qt.X, qt.Y, qt.Z]; 133 | 134 | %Broadcast a new odometry event with corresponding data 135 | odom_event_data = NewOdomEventData(agent_number, position, orientation); 136 | notify(obj, 'NewOdom', odom_event_data); 137 | 138 | end 139 | 140 | end 141 | end 142 | 143 | -------------------------------------------------------------------------------- /interfaces/kr_matlab_interface/example_interface.m: -------------------------------------------------------------------------------- 1 | function example_interface(hostname, n_agents) 2 | 3 | % pass hostname as 'localhost' for simulator running on same maching 4 | if nargin < 2 5 | error('Need hostname and n_agents as argument') 6 | return; 7 | end 8 | 9 | if(~isnumeric(n_agents)) 10 | disp('n_agents has to be an integer') 11 | return; 12 | end 13 | 14 | if n_agents < 1 15 | n_agents = 1; 16 | end 17 | 18 | if(~ischar(hostname)) 19 | disp('hostname has to be a string, setting to localhost') 20 | hostname = 'localhost'; 21 | end 22 | 23 | %Prepare plotting 24 | fig1 = figure; 25 | ax1 = axes('XLim',[-10, 10], 'YLim', [-10, 10], 'ZLim', [-10,10], 'Parent', fig1); 26 | 27 | quad_obj = QuadControlRos(hostname,n_agents, 'dragonfly'); 28 | 29 | %Create mesh visualization handles for each agent 30 | for n_ag = 1:n_agents 31 | vis_handles.ax_handles(n_ag) = plotTransforms([rand(1), rand(1), rand(1)], [1,0,0,0], 'MeshFilePath','multirotor.stl', 'MeshColor', [rand(1) rand(1) rand(1)], 'FrameSize', 2, 'Parent', ax1); 32 | end 33 | 34 | %Add event listner to update Mesh pose on new odometry 35 | new_odom_listner_handle = addlistener(quad_obj,'NewOdom',@(quad_obj,evnt)odomEventcallbackMethod(quad_obj,evnt,vis_handles)); 36 | 37 | %Turn on motors and takeoff 38 | for n_ag = 1:n_agents 39 | resp = quad_obj.motors(n_ag, 1); 40 | pause(0.1) %Might need longer pause on real platforms for motors to idle 41 | resp = quad_obj.takeoff(n_ag); 42 | end 43 | 44 | for i=1:30 45 | 46 | agent_num = randi([1, n_agents]); 47 | curr_odom = quad_obj.getOdom(agent_num); 48 | if ~isempty(curr_odom) 49 | curr_position = curr_odom.Pose.Pose.Position; 50 | txt = sprintf('%d pose %g %g %g %g',agent_num, curr_position.X, curr_position.Y, curr_position.Z); 51 | disp(txt); 52 | end 53 | quad_obj.send_twist(agent_num, [rand(1),rand(1),0,0]) 54 | 55 | pause(0.4) 56 | end 57 | 58 | %Hover all robots with zero vel 59 | for n=1:n_agents 60 | quad_obj.send_zero_twist(n); 61 | end 62 | 63 | % send to wp 64 | for n=1:n_agents 65 | response = quad_obj.send_wp(n, [randi([0,2]), randi([0,2]),0,1.0]); 66 | end 67 | 68 | clear quad_obj 69 | end 70 | 71 | function odomEventcallbackMethod(src,evnt, vis_handles) 72 | agent_number = evnt.agent_number; 73 | position = evnt.position; 74 | orientation = evnt.orientation; 75 | 76 | if ~isempty(vis_handles) 77 | %Take a look at 78 | %'R2019a/toolbox/robotics/robotcore/+robotics/+core/+internal/+visualization/TransformPainter.m' 79 | % move method in above file 80 | 81 | ax = vis_handles.ax_handles(agent_number); 82 | hBodyToInertial = findobj(ax, 'Type', 'hgtransform','Tag', robotics.core.internal.visualization.TransformPainter.GraphicsObjectTags.BodyToInertial); 83 | 84 | if ~isempty(hBodyToInertial) 85 | tform = quat2tform(orientation); 86 | tform(1:3,4) = position; 87 | set(hBodyToInertial(agent_number), 'Matrix', tform); 88 | end 89 | end 90 | end -------------------------------------------------------------------------------- /interfaces/kr_matlab_interface/kr_mav_manager.patch.package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_manager 3 | 1.0.0 4 | 5 | This package provides tools to interface with the kr_mav_control stack 6 | 7 | Justin Thomas 8 | BSD 9 | 10 | 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_msgs 16 | nav_msgs 17 | std_msgs 18 | sensor_msgs 19 | kr_mav_msgs 20 | kr_tracker_msgs 21 | message_generation 22 | std_srvs 23 | 24 | 25 | roscpp 26 | geometry_msgs 27 | nav_msgs 28 | std_msgs 29 | sensor_msgs 30 | kr_mav_msgs 31 | kr_tracker_msgs 32 | message_runtime 33 | std_srvs 34 | 35 | -------------------------------------------------------------------------------- /interfaces/kr_matlab_interface/kr_mav_msgs.patch.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_msgs 3 | 1.0.0 4 | kr_mav_msgs 5 | Kartik Mohta 6 | Michael Watterson 7 | Justin Thomas 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/kr_mav_msgs 12 | 13 | 14 | Kartik Mohta 15 | 16 | 17 | catkin 18 | 19 | 20 | message_generation 21 | geometry_msgs 22 | nav_msgs 23 | std_msgs 24 | 25 | 26 | 27 | message_runtime 28 | geometry_msgs 29 | nav_msgs 30 | std_msgs 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /interfaces/kr_mavros_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mavros_interface) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS roscpp 17 | nav_msgs 18 | geometry_msgs 19 | kr_mav_msgs 20 | nodelet) 21 | find_package(Eigen3 REQUIRED) 22 | 23 | # If mavros_msgs not found, don't build but do not cause a build error 24 | find_package(mavros_msgs) 25 | 26 | if(NOT ${mavros_msgs_FOUND}) 27 | message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface") 28 | else() 29 | include_directories(${catkin_INCLUDE_DIRS} ${mavros_msgs_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS 33 | LIBRARIES 34 | ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | roscpp 37 | nav_msgs 38 | geometry_msgs 39 | kr_mav_msgs 40 | mavros_msgs 41 | nodelet 42 | DEPENDS 43 | EIGEN3) 44 | 45 | add_library(${PROJECT_NAME} src/so3cmd_to_mavros_nodelet.cpp) 46 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 47 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 48 | 49 | install( 50 | TARGETS ${PROJECT_NAME} 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 54 | 55 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 56 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 57 | endif() 58 | -------------------------------------------------------------------------------- /interfaces/kr_mavros_interface/README.md: -------------------------------------------------------------------------------- 1 | # kr_mavros_interface 2 | 3 | This node translates `kr_mav_msgs/SO3Command` to `mavros_msgs/AttitudeTarget`. 4 | 5 | Please note that your flight controller yaw orientation estimation may be 6 | different to your odom system orientation estimation. We should transform our 7 | `kr_mav_msgs/SO3Command` into the flight controller yaw before publishing it. 8 | 9 | #### `mavros_msgs` requirement 10 | 11 | This node requires `mavros_msgs` to be present when building. If `mavros_msgs` is not found when building the first time, a warning is given and nothing in this package is built. If `mavros_msgs` is installed after this, force a recheck by adding `--force-cmake` to the `catkin_make`/`catkin build` command. 12 | -------------------------------------------------------------------------------- /interfaces/kr_mavros_interface/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /interfaces/kr_mavros_interface/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This reformats an so3_command and publishes it on compatabile mavros topics 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /interfaces/kr_mavros_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mavros_interface 3 | 1.0.0 4 | kr_mavros_interface 5 | Anurag Makineni 6 | Justin Thomas 7 | Kartik Mohta 8 | 9 | BSD 10 | 11 | 12 | catkin 13 | 14 | 15 | roscpp 16 | nav_msgs 17 | geometry_msgs 18 | kr_mav_msgs 19 | mavros_msgs 20 | nodelet 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /interfaces/kr_python_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kr_python_interface) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_python_setup() 6 | 7 | catkin_package() 8 | 9 | install(PROGRAMS src/kr_python_interface/mav_interface.py 10 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 11 | ) 12 | -------------------------------------------------------------------------------- /interfaces/kr_python_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_python_interface 3 | 0.1.0 4 | Provides a python interface for kr_mav_control 5 | Dinesh Thakur 6 | 7 | BSD 8 | 9 | Dinesh Thakur 10 | Laura Jarin-Lipschitz 11 | 12 | catkin 13 | 14 | rospy 15 | geometry_msgs 16 | kr_tracker_msgs 17 | kr_mav_manager 18 | 19 | 20 | -------------------------------------------------------------------------------- /interfaces/kr_python_interface/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['kr_python_interface'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /interfaces/kr_python_interface/src/kr_python_interface/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/interfaces/kr_python_interface/src/kr_python_interface/__init__.py -------------------------------------------------------------------------------- /interfaces/kr_python_interface/src/kr_python_interface/mav_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | import random 5 | 6 | from geometry_msgs.msg import Twist, Pose 7 | from nav_msgs.msg import Odometry 8 | from std_srvs.srv import SetBool, Trigger 9 | from kr_tracker_msgs.msg import TrajectoryTrackerAction, TrajectoryTrackerGoal, CircleTrackerAction, CircleTrackerGoal 10 | 11 | from kr_python_interface.mav_interface import KrMavInterface 12 | 13 | def main(): 14 | rospy.init_node('mav_example', anonymous=True) 15 | 16 | # Creating MAV objects 17 | mav_namespace = 'dragonfly' 18 | mav_id = 1 19 | mav_obj = KrMavInterface('dragonfly', mav_id) 20 | 21 | # Motor On / Take Off 22 | mav_obj.motors_on() 23 | mav_obj.take_off() 24 | 25 | rospy.sleep(1) 26 | 27 | #Send waypoint (open loop, have to sleep until finished) 28 | mav_obj.send_wp(4.0, 0.0, 1.0, 0.0) 29 | rospy.sleep(4) 30 | 31 | #Send waypoint blocking 32 | mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 0.5, 0.3, False) #x, y, z, yaw, vel, acc, relative 33 | 34 | #Send random twist commands 35 | for i in range(20): 36 | #get current odometry 37 | curr_odom = mav_obj.get_odom(); 38 | curr_position = curr_odom.pose.pose.position; 39 | rospy.loginfo('pose %g %g %g', curr_position.x, curr_position.y, curr_position.z); 40 | 41 | mav_obj.set_vel(random.uniform(0.1,1.0), random.uniform(0.1,1.0), 0, 0, 0, 0) #vx, vy, vz, 42 | 43 | rospy.sleep(0.3) 44 | 45 | #Send waypoint blocking 46 | mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 1.0, 0.5, False) #x, y, z, yaw, vel, acc, relative 47 | 48 | #Run circle tracker 49 | mav_obj.hover() 50 | goal = CircleTrackerGoal() 51 | goal.Ax = -1.0 52 | goal.Ay = -1.0 53 | goal.T = 4.0 54 | num_repetitions = 1 55 | goal.duration = goal.T*num_repetitions 56 | mav_obj.circle_tracker_client.cancel_all_goals() 57 | rospy.sleep(0.1) 58 | mav_obj.circle_tracker_client.send_goal(goal) 59 | rospy.logwarn("Send circle") 60 | 61 | success = mav_obj.transition_service_call('CircleTracker') 62 | if not success: 63 | rospy.logwarn("Failed to transition to circle tracker (is there an active goal?)") 64 | 65 | rospy.logwarn("Waiting for circle to run") 66 | mav_obj.circle_tracker_client.wait_for_result() 67 | 68 | #Send waypoint blocking 69 | mav_obj.send_wp_block(0.0, 0.0, 1.5, 0.0, 1.0, 0.5, False) #x, y, z, yaw, vel, acc, relative 70 | 71 | #Send multiple waypoints by fitting traj 72 | goal = TrajectoryTrackerGoal() 73 | wp = Pose() 74 | wp.position.x = 0.0 75 | wp.position.y = 0.0 76 | wp.position.z = 2.0 77 | goal.waypoints.append(wp) 78 | 79 | wp1 = Pose() 80 | wp1.position.x = 0.0 81 | wp1.position.y = 1.0 82 | wp1.position.z = 1.5 83 | goal.waypoints.append(wp1) 84 | 85 | wp2 = Pose() 86 | wp2.position.x = 2.0 87 | wp2.position.y = 2.0 88 | wp2.position.z = 1.0 89 | goal.waypoints.append(wp2) 90 | 91 | wp3 = Pose() 92 | wp3.position.x = 3.0 93 | wp3.position.y = 0.0 94 | wp3.position.z = 0.5 95 | goal.waypoints.append(wp3) 96 | 97 | mav_obj.traj_tracker_client.send_goal(goal) 98 | success = mav_obj.transition_service_call('TrajectoryTracker') 99 | if not success: 100 | rospy.logwarn("Failed to transition to trajectory tracker (is there an active goal?)") 101 | 102 | rospy.logwarn("Waiting for traj to run") 103 | mav_obj.traj_tracker_client.wait_for_result() 104 | 105 | #Send waypoint blocking 106 | mav_obj.send_wp_block(0.0, 0.0, 1.0, 0.0, 1.5, 0.5, False) #x, y, z, yaw, vel, acc, relative 107 | 108 | # Land / Motors off 109 | mav_obj.land() 110 | rospy.sleep(3) 111 | mav_obj.motors_off() 112 | 113 | if __name__ == '__main__': 114 | try : 115 | main() 116 | except rospy.ROSInterruptException : 117 | pass 118 | -------------------------------------------------------------------------------- /interfaces/kr_python_interface/src/kr_python_interface/mav_interface.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | from __future__ import print_function 3 | 4 | import rospy 5 | import actionlib 6 | 7 | from actionlib_msgs.msg import GoalStatus 8 | from geometry_msgs.msg import Twist 9 | from nav_msgs.msg import Odometry, Path 10 | from kr_tracker_msgs.msg import TrajectoryTrackerAction, TrajectoryTrackerGoal, CircleTrackerAction, CircleTrackerGoal, LineTrackerAction, LineTrackerGoal 11 | 12 | from kr_tracker_msgs.srv import Transition 13 | from std_srvs.srv import Trigger, SetBool 14 | from kr_mav_manager.srv import Vec4 15 | 16 | class KrMavInterface(object): 17 | 18 | def __init__(self, mav_namespace='dragonfly', id=0): 19 | self.mav_namespace = mav_namespace 20 | self.id = id 21 | 22 | self.mav_name = self.mav_namespace + str(self.id) 23 | 24 | self.pub_vel = rospy.Publisher('/' + self.mav_name + '/cmd_vel', Twist, queue_size=10) 25 | 26 | self.odom = Odometry() 27 | self.sub_odom = rospy.Subscriber('/' + self.mav_name + '/odom', Odometry, self.update_odom) 28 | 29 | self.line_tracker_client = actionlib.SimpleActionClient(self.mav_name + 30 | '/trackers_manager/line_tracker_min_jerk/LineTracker', LineTrackerAction) 31 | self.traj_tracker_client = actionlib.SimpleActionClient(self.mav_name + 32 | '/trackers_manager/trajectory_tracker/TrajectoryTracker', TrajectoryTrackerAction) 33 | self.circle_tracker_client = actionlib.SimpleActionClient(self.mav_name + 34 | '/trackers_manager/circle_tracker/CircleTracker', CircleTrackerAction) 35 | self.traj_tracker_status = "" 36 | 37 | def update_odom(self, msg): 38 | self.odom = msg 39 | 40 | def get_odom(self): 41 | return self.odom 42 | 43 | def motors_on(self): 44 | try: 45 | motors = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/motors', SetBool) 46 | resp = motors(True) 47 | rospy.loginfo(resp) 48 | except rospy.ServiceException as e: 49 | rospy.logwarn("Service call failed: %s"%e) 50 | return 'aborted' 51 | 52 | def motors_off(self): 53 | try: 54 | motors = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/motors', SetBool) 55 | resp = motors(False) 56 | rospy.loginfo(resp) 57 | except rospy.ServiceException as e: 58 | rospy.logwarn("Service call failed: %s"%e) 59 | return 'aborted' 60 | 61 | def take_off(self): 62 | try: 63 | takeoff = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/takeoff', Trigger) 64 | resp = takeoff() 65 | rospy.loginfo(resp) 66 | except rospy.ServiceException as e: 67 | rospy.logwarn("Service call failed: %s"%e) 68 | return 'aborted' 69 | 70 | def hover(self): 71 | rospy.logwarn("Transition to hover") 72 | self.traj_tracker_client.cancel_all_goals() 73 | rospy.wait_for_service('/' + self.mav_name + '/mav_services/hover') 74 | try: 75 | srv = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/hover', Trigger) 76 | resp = srv() 77 | rospy.loginfo(resp) 78 | except rospy.ServiceException as e: 79 | rospy.logwarn("Service call failed: %s" % e) 80 | return False 81 | 82 | def land(self): 83 | rospy.logwarn("Transition to land") 84 | self.traj_tracker_client.cancel_all_goals() 85 | rospy.wait_for_service('/' + self.mav_name + '/mav_services/land') 86 | try: 87 | srv = rospy.ServiceProxy('/' + self.mav_name + '/mav_services/land', Trigger) 88 | resp = srv() 89 | rospy.loginfo(resp) 90 | except rospy.ServiceException as e: 91 | rospy.logwarn("Service call failed: %s" % e) 92 | return False 93 | 94 | def set_vel(self, vx=0.0, vy=0.0, vz=0.0, ax=0.0, ay=0.0, az=0.0): 95 | command = Twist() 96 | command.linear.x = vx 97 | command.linear.y = vy 98 | command.linear.z = vz 99 | command.angular.x = ax 100 | command.angular.y = ay 101 | command.angular.z = az 102 | 103 | self.pub_vel.publish(command) 104 | 105 | def send_wp(self, x, y, z, yaw): 106 | try: 107 | rospy.wait_for_service('/' + self.mav_name + '/mav_services/goTo') 108 | srv = rospy.ServiceProxy('/' + self.mav_name +'/mav_services/goTo', Vec4) 109 | resp = srv([x, y, z, yaw]) 110 | rospy.loginfo(resp) 111 | except rospy.ServiceException as e: 112 | rospy.logwarn("Service call failed: %s" % e) 113 | return False 114 | 115 | def send_wp_block(self, x, y, z, yaw, vel=1.0, acc=0.5, relative=False): 116 | goal = LineTrackerGoal(); 117 | goal.x = x; 118 | goal.y = y; 119 | goal.z = z; 120 | goal.yaw = yaw; 121 | goal.v_des = 1.0; 122 | goal.a_des = 0.5; 123 | goal.relative = False; 124 | 125 | self.line_tracker_client.send_goal(goal) 126 | self.transition_service_call('LineTrackerMinJerk') 127 | rospy.logwarn("Waiting for Line Tracker to complete") 128 | self.line_tracker_client.wait_for_result() 129 | 130 | def transition_service_call(self, tracker_name): 131 | rospy.loginfo('waiting for transition service for ' + tracker_name) 132 | rospy.wait_for_service('/' + self.mav_name +'/trackers_manager/transition') 133 | try: 134 | tt = rospy.ServiceProxy('/' + self.mav_name +'/trackers_manager/transition', Transition) 135 | resp = tt('kr_trackers/' + tracker_name) 136 | rospy.loginfo(resp) 137 | if resp.success == False or "already active" in resp.message: 138 | return False 139 | return True 140 | except rospy.ServiceException as e: 141 | rospy.logwarn("Service call failed: %s" % e) 142 | return False 143 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_qualcomm_interface) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/") 15 | 16 | find_package( 17 | catkin REQUIRED 18 | COMPONENTS roscpp 19 | kr_mav_msgs 20 | nav_msgs 21 | nodelet 22 | tf_conversions) 23 | find_package(Eigen3 REQUIRED) 24 | 25 | find_package(Snav) 26 | if(NOT ${Snav_FOUND}) 27 | message(WARNING "NOTE: snav not found so not building kr_qualcomm_interface") 28 | else() 29 | include_directories(${Snav_INCLUDE_DIR}) 30 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) 31 | 32 | catkin_package( 33 | LIBRARIES 34 | ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | roscpp 37 | kr_mav_msgs 38 | nav_msgs 39 | nodelet 40 | tf_conversions 41 | DEPENDS 42 | EIGEN3) 43 | 44 | add_library(${PROJECT_NAME} src/so3cmd_to_qualcomm_nodelet.cpp src/trpycmd_to_snav_nodelet.cpp 45 | src/poscmd_to_snav_nodelet.cpp) 46 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 47 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Snav_LIBRARY}) 48 | 49 | add_executable(vio_odom_publisher src/vio_odom_publisher.cpp) 50 | target_link_libraries(vio_odom_publisher ${catkin_LIBRARIES} ${Snav_LIBRARY}) 51 | 52 | add_executable(snav_publisher src/snav_publisher.cpp) 53 | add_dependencies(snav_publisher ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(snav_publisher ${catkin_LIBRARIES} ${Snav_LIBRARY}) 55 | 56 | install( 57 | TARGETS ${PROJECT_NAME} vio_odom_publisher snav_publisher 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 61 | 62 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 63 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 64 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 65 | 66 | endif() 67 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/cmake/FindSnav.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(Snav_INCLUDE_DIR 2 | NAMES 3 | snav/snapdragon_navigator.h 4 | PATHS 5 | /usr/include 6 | NO_DEFAULT_PATH 7 | ) 8 | 9 | FIND_LIBRARY(Snav_LIBRARY 10 | NAMES 11 | snav_arm 12 | PATHS 13 | /usr/lib 14 | NO_DEFAULT_PATH 15 | ) 16 | 17 | IF(Snav_INCLUDE_DIR AND Snav_LIBRARY) 18 | SET(Snav_FOUND TRUE) 19 | ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY) 20 | SET(Snav_FOUND FALSE) 21 | ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY) 22 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/config/gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 3.4, y: 3.4, z: 5.4} 3 | vel: {x: 2.8, y: 2.8, z: 3.0} 4 | ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} 5 | kib: {x: 0.00, y: 0.00, z: 0.00} 6 | rot: {x: 1.5, y: 1.5, z: 1.0} 7 | ang: {x: 0.13, y: 0.13, z: 0.1} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/config/mav_manager_params.yaml: -------------------------------------------------------------------------------- 1 | server_wait_timeout: 0.5 2 | need_imu: false 3 | need_output_data: true 4 | use_attitude_safety_catch: true 5 | max_attitude_angle: 0.43 6 | takeoff_height: 0.2 7 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/config/tracker_params.yaml: -------------------------------------------------------------------------------- 1 | # This should contain tracker parameters 2 | 3 | line_tracker_distance: 4 | default_v_des: 1.0 5 | default_a_des: 0.5 6 | epsilon: 0.1 7 | 8 | line_tracker_min_jerk: 9 | default_v_des: 1.0 10 | default_a_des: 0.5 11 | default_yaw_v_des: 0.8 12 | default_yaw_a_des: 0.2 13 | 14 | trajectory_tracker: 15 | max_vel_des: 0.5 16 | max_acc_des: 0.3 17 | 18 | velocity_tracker: 19 | timeout: 0.5 20 | 21 | lissajous_tracker: 22 | frame_id: odom 23 | 24 | lissajous_adder: 25 | frame_id: odom -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/config/trackers.yaml: -------------------------------------------------------------------------------- 1 | trackers: 2 | - kr_trackers/LineTrackerMinJerk 3 | - kr_trackers/LineTrackerDistance 4 | # - kr_trackers/LineTrackerTrapezoid 5 | # - kr_trackers/LineTrackerYaw 6 | - kr_trackers/VelocityTrackerAction 7 | - kr_trackers/NullTracker 8 | - kr_trackers/CircleTracker 9 | - kr_trackers/TrajectoryTracker 10 | - kr_trackers/SmoothVelTracker 11 | - kr_trackers/LissajousTracker 12 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/launch/snav_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 15 | 16 | 17 | 18 | 23 | 24 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/launch/snav_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This take a so3_command and publishes it to the robot using Qualcomm's API 5 | 6 | 7 | 8 | 9 | This take a trpy_command and publishes it to the robot using Qualcomm's Snav API 10 | 11 | 12 | 13 | 14 | This take a pos_command and publishes it to the robot using Qualcomm's Snav API 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr_qualcomm_interface 4 | 0.0.0 5 | The kr_qualcomm_interface package 6 | 7 | Dinesh Thakur 8 | 9 | BSD 10 | 11 | catkin 12 | nav_msgs 13 | nodelet 14 | kr_mav_msgs 15 | roscpp 16 | tf_conversions 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /interfaces/kr_qualcomm_interface/src/vio_odom_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main(int argc, char *argv[]) 7 | { 8 | ros::init(argc, argv, "vio_odom_publisher"); 9 | ros::NodeHandle nh; 10 | ros::Publisher odom_pub = nh.advertise("odom", 10); 11 | ros::Rate loop_rate(100); 12 | 13 | int update_ret; 14 | float roll, pitch, yaw; 15 | while(ros::ok()) 16 | { 17 | // read the flight data 18 | SnavCachedData *snav_data = NULL; 19 | if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_data) != 0) 20 | { 21 | ROS_ERROR("failed to get flight data ptr"); 22 | continue; 23 | } 24 | update_ret = sn_update_data(); 25 | if(update_ret != 0) 26 | { 27 | ROS_ERROR("detected likely failure in snav, ensure it is running"); 28 | continue; 29 | } 30 | else 31 | { 32 | nav_msgs::Odometry odom; 33 | odom.header.stamp = ros::Time::now(); 34 | odom.header.frame_id = "odom"; 35 | 36 | // set the position 37 | odom.pose.pose.position.x = snav_data->vio_pos_vel.position_estimated[0]; 38 | odom.pose.pose.position.y = snav_data->vio_pos_vel.position_estimated[1]; 39 | odom.pose.pose.position.z = snav_data->vio_pos_vel.position_estimated[2]; 40 | // set the attitude 41 | roll = snav_data->attitude_estimate.roll; 42 | pitch = snav_data->attitude_estimate.pitch; 43 | yaw = snav_data->attitude_estimate.yaw; 44 | // convert to quaternion 45 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); 46 | odom.pose.pose.orientation = odom_quat; 47 | 48 | // set the velocity 49 | odom.child_frame_id = "base_link"; 50 | odom.twist.twist.linear.x = snav_data->vio_pos_vel.velocity_estimated[0]; 51 | odom.twist.twist.linear.y = snav_data->vio_pos_vel.velocity_estimated[1]; 52 | odom.twist.twist.linear.z = snav_data->vio_pos_vel.velocity_estimated[2]; 53 | 54 | // publish the message 55 | odom_pub.publish(odom); 56 | 57 | ros::spinOnce(); 58 | loop_rate.sleep(); 59 | } 60 | } 61 | return 0; 62 | } -------------------------------------------------------------------------------- /interfaces/kr_rosflight_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_rosflight_interface) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS roscpp 17 | nav_msgs 18 | geometry_msgs 19 | kr_mav_msgs 20 | nodelet) 21 | find_package(Eigen3 REQUIRED) 22 | 23 | # If rosflight_msgs not found, don't build but do not cause a build error 24 | find_package(rosflight_msgs) 25 | 26 | if(NOT ${rosflight_msgs_FOUND}) 27 | message(WARNING "NOTE: rosflight_msgs not found so not building kr_rosflight_interface") 28 | else() 29 | catkin_package( 30 | LIBRARIES 31 | ${PROJECT_NAME} 32 | CATKIN_DEPENDS 33 | roscpp 34 | nav_msgs 35 | geometry_msgs 36 | kr_mav_msgs 37 | rosflight_msgs 38 | nodelet 39 | DEPENDS 40 | EIGEN3) 41 | 42 | add_library(${PROJECT_NAME} src/so3cmd_to_rosflight_nodelet.cpp) 43 | target_include_directories(${PROJECT_NAME} PUBLIC ${caktin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} 44 | ${rosflight_msgs_INCLUDE_DIRS}) 45 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 46 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 47 | 48 | install( 49 | TARGETS ${PROJECT_NAME} 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 53 | 54 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 55 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 56 | endif() 57 | -------------------------------------------------------------------------------- /interfaces/kr_rosflight_interface/README.md: -------------------------------------------------------------------------------- 1 | # kr_rosflight_interface 2 | 3 | This node translates `kr_mav_msgs/SO3Command` to `rosflight_msgs/Command`. 4 | 5 | #### `rosflight_msgs` requirement 6 | 7 | This node requires `rosflight_msgs` to be present when building. If `rosflight_msgs` is not found when building the first time, a warning is given and nothing in this package is built. If `rosflight_msgs` is installed after this, force a recheck by adding `--force-cmake` to the `catkin_make`/`catkin build` command. 8 | -------------------------------------------------------------------------------- /interfaces/kr_rosflight_interface/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /interfaces/kr_rosflight_interface/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This reformats an so3_command and publishes it on compatabile rosflight topics 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /interfaces/kr_rosflight_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_rosflight_interface 3 | 0.1.0 4 | kr_rosflight_interface 5 | 6 | Jon Fink 7 | BSD 8 | 9 | 10 | catkin 11 | 12 | 13 | roscpp 14 | nav_msgs 15 | geometry_msgs 16 | kr_mav_msgs 17 | rosflight_msgs 18 | nodelet 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_serial_interface) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package(catkin REQUIRED COMPONENTS nodelet kr_mav_msgs roscpp sensor_msgs) 15 | 16 | include_directories(include ${catkin_INCLUDE_DIRS}) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | LIBRARIES 21 | ${PROJECT_NAME} 22 | CATKIN_DEPENDS 23 | nodelet 24 | kr_mav_msgs 25 | roscpp 26 | sensor_msgs 27 | DEPENDS) 28 | 29 | add_library( 30 | ${PROJECT_NAME} 31 | src/ASIOSerialDevice.cc 32 | src/asctec_serial_interface.cpp 33 | src/decode_msgs.cpp 34 | src/encode_msgs.cpp 35 | src/quad_decode_msg_nodelet.cpp 36 | src/quad_encode_msg.cpp 37 | src/quad_encode_msg_nodelet.cpp 38 | src/quad_serial_comm_nodelet.cpp) 39 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 40 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 41 | 42 | install( 43 | TARGETS ${PROJECT_NAME} 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 47 | 48 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 49 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/include/kr_serial_interface/ASIOSerialDevice.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of asio_serial_device, a class wrapper to 3 | use the boost::asio serial functionality. 4 | 5 | asio_serial_device is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | 18 | Nathan Michael, Aug. 2011 19 | */ 20 | 21 | #ifndef ASIOSERIALDEVICE_H 22 | #define ASIOSERIALDEVICE_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // Class version of example ASIO over serial with boost::asio: 32 | // http://groups.google.com/group/boost-list/browse_thread/thread/5cc7dcc7b90d41fc 33 | 34 | #define MAX_READ_LENGTH 16 35 | 36 | namespace ba = boost::asio; 37 | 38 | class ASIOSerialDevice 39 | { 40 | public: 41 | ASIOSerialDevice(); 42 | ASIOSerialDevice(const std::string &device, unsigned int baud); 43 | ~ASIOSerialDevice(); 44 | 45 | void SetReadCallback(const boost::function &handler); 46 | 47 | void Start(); 48 | void Stop(); 49 | void Close(); 50 | 51 | void Read(); 52 | bool Write(const std::vector &msg); 53 | void Open( 54 | const std::string &device_, unsigned int baud_, 55 | ba::serial_port_base::parity parity = ba::serial_port_base::parity(ba::serial_port_base::parity::none), 56 | ba::serial_port_base::character_size csize = ba::serial_port_base::character_size(8), 57 | ba::serial_port_base::flow_control flow = 58 | ba::serial_port_base::flow_control(ba::serial_port_base::flow_control::none), 59 | ba::serial_port_base::stop_bits stop = ba::serial_port_base::stop_bits(ba::serial_port_base::stop_bits::one)); 60 | 61 | bool Active(); 62 | 63 | private: 64 | void CloseCallback(const boost::system::error_code &error); 65 | 66 | void ReadStart(); 67 | void ReadComplete(const boost::system::error_code &error, size_t bytes_transferred); 68 | 69 | void WriteCallback(const std::vector &msg); 70 | void WriteStart(); 71 | void WriteComplete(const boost::system::error_code &error); 72 | 73 | std::string device; 74 | unsigned int baud; 75 | bool async_active, open; 76 | std::deque > write_msgs; 77 | 78 | boost::thread thread; 79 | ba::io_service io_service; 80 | ba::serial_port *serial_port; 81 | boost::function read_callback; 82 | 83 | unsigned char read_msg[MAX_READ_LENGTH]; 84 | }; 85 | #endif 86 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/include/kr_serial_interface/comm_types.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADROTOR_MSGS_COMM_TYPES_H 2 | #define QUADROTOR_MSGS_COMM_TYPES_H 3 | 4 | #include 5 | 6 | #define TYPE_SO3_CMD 's' 7 | struct SO3_CMD_INPUT 8 | { 9 | // Scaling factors when decoding 10 | int16_t force[3]; // /500 (range: +-65 N on each axis) 11 | int8_t des_qx, des_qy, des_qz, des_qw; // /125 12 | int16_t angvel_x, angvel_y, angvel_z; // /1000 (range: +-32 rad/s) 13 | uint8_t kR[3]; // /50 (range: 0-5.1) 14 | uint8_t kOm[3]; // /100 (range: 0-2.5) 15 | int16_t cur_yaw; // /1e4 16 | int16_t kf_correction; // /1e11 (range: +-3.2e-7) 17 | int8_t angle_corrections[2]; // roll,pitch /2500 (range: +-0.05 rad) 18 | uint8_t enable_motors : 1; 19 | uint8_t use_external_yaw : 1; 20 | uint8_t seq; 21 | }; 22 | 23 | #define TYPE_STATUS_DATA 'c' 24 | struct STATUS_DATA 25 | { 26 | uint16_t loop_rate; 27 | uint16_t voltage; 28 | uint8_t seq; 29 | }; 30 | 31 | #define TYPE_OUTPUT_DATA 'd' 32 | struct OUTPUT_DATA 33 | { 34 | uint16_t loop_rate; 35 | uint16_t voltage; 36 | int16_t roll, pitch, yaw; 37 | int16_t ang_vel[3]; 38 | int16_t acc[3]; 39 | int16_t dheight; 40 | int32_t height; 41 | int16_t mag[3]; 42 | uint8_t radio[8]; 43 | uint8_t rpm[4]; 44 | uint8_t seq; 45 | }; 46 | 47 | #define TYPE_TRPY_CMD 'p' 48 | struct TRPY_CMD 49 | { 50 | int16_t thrust; 51 | int16_t roll; 52 | int16_t pitch; 53 | int16_t yaw; 54 | int16_t current_yaw; 55 | uint8_t enable_motors : 1; 56 | uint8_t use_external_yaw : 1; 57 | }; 58 | 59 | #define TYPE_PWM_CMD 'w' 60 | struct PWM_CMD_INPUT 61 | { 62 | // Scaling factors when decoding 63 | uint8_t pwm[2]; // /255 64 | }; 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/include/kr_serial_interface/decode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADROTOR_MSGS_DECODE_MSGS_H 2 | #define QUADROTOR_MSGS_DECODE_MSGS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace kr_mav_msgs 11 | { 12 | bool decodeOutputData(const std::vector &data, kr_mav_msgs::OutputData &output); 13 | 14 | bool decodeStatusData(const std::vector &data, kr_mav_msgs::StatusData &status); 15 | } // namespace kr_mav_msgs 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/include/kr_serial_interface/encode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADROTOR_MSGS_ENCODE_MSGS_H 2 | #define QUADROTOR_MSGS_ENCODE_MSGS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace kr_mav_msgs 12 | { 13 | void encodeSO3Command(const kr_mav_msgs::SO3Command &so3_command, std::vector &output); 14 | void encodeTRPYCommand(const kr_mav_msgs::TRPYCommand &trpy_command, std::vector &output); 15 | void encodePWMCommand(const kr_mav_msgs::PWMCommand &pwm_command, std::vector &output); 16 | } // namespace kr_mav_msgs 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/include/kr_serial_interface/serial_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_SERIAL_COMM_SERIAL_INTERFACE_H 2 | #define QUAD_SERIAL_COMM_SERIAL_INTERFACE_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | void encode_serial_msg(const kr_mav_msgs::Serial &msg, std::vector &serial_data); 10 | 11 | void process_serial_data(const uint8_t *data, const size_t count, 12 | boost::function callback); 13 | #endif 14 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This packs the data being sent to the quad into a nice efficient byte-stream. 5 | 6 | 7 | 8 | 9 | 10 | This unpacks the data being sent from the quad into a ROS message. 11 | 12 | 13 | 14 | 15 | 16 | This sends the packed data over the serial port. It also receives data from the quad and unpacks the payload from the received packet. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_serial_interface 3 | 1.0.0 4 | kr_serial_interface 5 | GPL license since asio_serial_device is under GPL and we link to it. 6 | Kartik Mohta 7 | Michael Watterson 8 | 9 | GPL-3.0 10 | 11 | Kartik Mohta 12 | 13 | 14 | catkin 15 | 16 | 17 | nodelet 18 | kr_mav_msgs 19 | roscpp 20 | sensor_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/asctec_serial_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /* Packet format: 4 | * Packet: | 0x55 | 0x55 |count|type|---data---|crcL|crcH| 5 | * State: 0 1 2 3 4 5 6 6 | */ 7 | 8 | enum PacketState 9 | { 10 | kPacketStart1, 11 | kPacketStart2, 12 | kPacketCount, 13 | kPacketType, 14 | kPacketData, 15 | kPacketCRCL, 16 | kPacketCRCH, 17 | }; 18 | 19 | static const unsigned int kPacketOverhead = 6; 20 | static const unsigned int kPacketDataMaxSize = 100; 21 | static const uint8_t kPacketStartString[] = {0x55, 0x55}; 22 | 23 | static uint16_t crc16(const uint8_t *data, int count); 24 | static uint16_t crc_update(uint16_t crc, uint8_t data); 25 | 26 | void encode_serial_msg(const kr_mav_msgs::Serial &msg, std::vector &serial_data) 27 | { 28 | const uint8_t data_length = msg.data.size(); 29 | 30 | if(data_length > kPacketDataMaxSize) 31 | { 32 | std::cerr << "encode_serial_msg: Message too large: " << msg.data.size() << ", max: " << kPacketDataMaxSize 33 | << std::endl; 34 | return; 35 | } 36 | 37 | serial_data.resize(kPacketOverhead + data_length); 38 | serial_data[0] = kPacketStartString[0]; 39 | serial_data[1] = kPacketStartString[1]; 40 | serial_data[2] = data_length; 41 | serial_data[3] = msg.type; 42 | 43 | uint16_t crc = crc16(&data_length, 1); 44 | crc = crc_update(crc, msg.type); 45 | 46 | memcpy(&(serial_data[4]), &(msg.data[0]), data_length); 47 | for(int i = 0; i < data_length; i++) 48 | crc = crc_update(crc, msg.data[i]); 49 | 50 | serial_data[4 + data_length + 0] = crc & 0xFF; 51 | serial_data[4 + data_length + 1] = crc >> 8; 52 | } 53 | 54 | void process_serial_data(const uint8_t *data, const size_t count, boost::function callback) 55 | { 56 | static kr_mav_msgs::Serial serial_msg; 57 | static enum PacketState state = kPacketStart1; 58 | static uint16_t received_length = 0, data_count = 0; 59 | ; 60 | static uint16_t expected_crc = 0, received_crc = 0; 61 | 62 | for(size_t i = 0; i < count; i++) 63 | { 64 | const uint8_t c = data[i]; 65 | // printf("%d, %X\n", state, c); 66 | if(state == kPacketStart1 && c == kPacketStartString[0]) 67 | state = kPacketStart2; 68 | else if(state == kPacketStart2 && c == kPacketStartString[1]) 69 | state = kPacketCount; 70 | else if(state == kPacketCount) 71 | { 72 | received_length = c; 73 | if(received_length > kPacketDataMaxSize) 74 | state = kPacketStart1; 75 | else 76 | { 77 | expected_crc = crc16(&c, 1); 78 | serial_msg.data.resize(received_length); 79 | data_count = 0; 80 | state = kPacketType; 81 | } 82 | } 83 | else if(state == kPacketType) 84 | { 85 | serial_msg.type = c; 86 | expected_crc = crc_update(expected_crc, c); 87 | if(received_length > 0) 88 | state = kPacketData; 89 | else 90 | state = kPacketCRCL; 91 | } 92 | else if(state == kPacketData) 93 | { 94 | serial_msg.data[data_count] = c; 95 | expected_crc = crc_update(expected_crc, c); 96 | data_count++; 97 | if(data_count == received_length) 98 | state = kPacketCRCL; 99 | } 100 | else if(state == kPacketCRCL) 101 | { 102 | received_crc = c; 103 | state = kPacketCRCH; 104 | } 105 | else if(state == kPacketCRCH) 106 | { 107 | received_crc = received_crc + 256 * c; 108 | if(expected_crc == received_crc) 109 | { 110 | // Received complete packet 111 | callback(serial_msg); 112 | } 113 | state = kPacketStart1; 114 | } 115 | else 116 | state = kPacketStart1; 117 | } 118 | } 119 | 120 | // CRC-16-ITU-T 121 | static uint16_t crc_update(uint16_t crc, uint8_t data) 122 | { 123 | uint16_t x = ((crc >> 8) ^ data); 124 | x ^= x >> 4; 125 | 126 | crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x; 127 | 128 | return crc; 129 | } 130 | 131 | static uint16_t crc16(const uint8_t *data, int count) 132 | { 133 | uint16_t crc = 0xffff; 134 | 135 | for(uint16_t i = 0; i < count; i++) 136 | { 137 | crc = crc_update(crc, data[i]); 138 | } 139 | return crc; 140 | } 141 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/decode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace kr_mav_msgs 7 | { 8 | bool decodeOutputData(const std::vector &data, kr_mav_msgs::OutputData &output) 9 | { 10 | struct OUTPUT_DATA output_data; 11 | if(data.size() != sizeof(output_data)) 12 | return false; 13 | 14 | memcpy(&output_data, &data[0], sizeof(output_data)); 15 | output.loop_rate = output_data.loop_rate; 16 | output.voltage = output_data.voltage / 1e3f; 17 | 18 | const double roll = output_data.roll / 1e2f * M_PI / 180; 19 | const double pitch = output_data.pitch / 1e2f * M_PI / 180; 20 | const double yaw = output_data.yaw / 1e2f * M_PI / 180; 21 | // Asctec (2012 firmware) uses Z-Y-X convention 22 | Eigen::Quaternionf q = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * 23 | Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * 24 | Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()); 25 | output.orientation.w = q.w(); 26 | output.orientation.x = q.x(); 27 | output.orientation.y = q.y(); 28 | output.orientation.z = q.z(); 29 | 30 | output.angular_velocity.x = output_data.ang_vel[0] * 0.0154f * M_PI / 180; 31 | output.angular_velocity.y = output_data.ang_vel[1] * 0.0154f * M_PI / 180; 32 | output.angular_velocity.z = output_data.ang_vel[2] * 0.0154f * M_PI / 180; 33 | 34 | output.linear_acceleration.x = output_data.acc[0] / 1e3f * 9.81f; 35 | output.linear_acceleration.y = output_data.acc[1] / 1e3f * 9.81f; 36 | output.linear_acceleration.z = output_data.acc[2] / 1e3f * 9.81f; 37 | 38 | output.pressure_dheight = output_data.dheight / 1e3f; 39 | output.pressure_height = output_data.height / 1e3f; 40 | 41 | output.magnetic_field.x = output_data.mag[0] / 2500.0f; 42 | output.magnetic_field.y = output_data.mag[1] / 2500.0f; 43 | output.magnetic_field.z = output_data.mag[2] / 2500.0f; 44 | 45 | for(int i = 0; i < 8; i++) 46 | { 47 | output.radio_channel[i] = output_data.radio[i]; 48 | } 49 | 50 | // Asctec firmware uses the following rotor numbering convention: 51 | // *1* Front 52 | // 3 4 53 | // 2 54 | // 55 | // But we want: 56 | // *1* Front 57 | // 2 4 58 | // 3 59 | const int motors_map[] = {0, 2, 1, 3}; 60 | for(int i = 0; i < 4; i++) 61 | { 62 | // The following conversion is from 63 | // http://wiki.asctec.de/display/AR/List+of+all+predefined+variables%2C+commands+and+parameters 64 | // 65 | // motorRPM = 1075+m*37.625 66 | // 67 | // Note: If m == 0, the motors are not spinning. 68 | int m = output_data.rpm[motors_map[i]]; 69 | output.motor_rpm[i] = (m == 0 ? 0 : 1075) + m * 37.625; 70 | } 71 | 72 | output.seq = output_data.seq; 73 | 74 | return true; 75 | } 76 | 77 | bool decodeStatusData(const std::vector &data, kr_mav_msgs::StatusData &status) 78 | { 79 | struct STATUS_DATA status_data; 80 | if(data.size() != sizeof(status_data)) 81 | return false; 82 | memcpy(&status_data, &data[0], sizeof(status_data)); 83 | 84 | status.loop_rate = status_data.loop_rate; 85 | status.voltage = status_data.voltage / 1e3f; 86 | status.seq = status_data.seq; 87 | 88 | return true; 89 | } 90 | 91 | } // namespace kr_mav_msgs 92 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/encode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace kr_mav_msgs 9 | { 10 | namespace 11 | { 12 | template 13 | using removeref = typename std::remove_reference::type; 14 | 15 | template 16 | removeref saturation_cast(Source const src, char const *variable_name = "") 17 | { 18 | try 19 | { 20 | return boost::numeric_cast>(src); 21 | } 22 | catch(const boost::numeric::negative_overflow &e) 23 | { 24 | ROS_WARN("Negative overflow when setting %s", variable_name); 25 | return std::numeric_limits>::lowest(); 26 | } 27 | catch(const boost::numeric::positive_overflow &e) 28 | { 29 | ROS_WARN("Positive overflow when setting %s", variable_name); 30 | return std::numeric_limits>::max(); 31 | } 32 | } 33 | } // namespace 34 | 35 | // NOTE(Kartik): Macro needed in order to get the tgt variable name as a string 36 | #define SATURATE_CAST(tgt, src) tgt = saturation_cast(src, #tgt) 37 | 38 | void encodeSO3Command(const kr_mav_msgs::SO3Command &so3_command, std::vector &output) 39 | { 40 | struct SO3_CMD_INPUT so3_cmd_input; 41 | 42 | SATURATE_CAST(so3_cmd_input.force[0], so3_command.force.x * 500); 43 | SATURATE_CAST(so3_cmd_input.force[1], so3_command.force.y * 500); 44 | SATURATE_CAST(so3_cmd_input.force[2], so3_command.force.z * 500); 45 | 46 | SATURATE_CAST(so3_cmd_input.des_qx, so3_command.orientation.x * 125); 47 | SATURATE_CAST(so3_cmd_input.des_qy, so3_command.orientation.y * 125); 48 | SATURATE_CAST(so3_cmd_input.des_qz, so3_command.orientation.z * 125); 49 | SATURATE_CAST(so3_cmd_input.des_qw, so3_command.orientation.w * 125); 50 | 51 | SATURATE_CAST(so3_cmd_input.angvel_x, so3_command.angular_velocity.x * 1000); 52 | SATURATE_CAST(so3_cmd_input.angvel_y, so3_command.angular_velocity.y * 1000); 53 | SATURATE_CAST(so3_cmd_input.angvel_z, so3_command.angular_velocity.z * 1000); 54 | 55 | SATURATE_CAST(so3_cmd_input.kR[0], so3_command.kR[0] * 50); 56 | SATURATE_CAST(so3_cmd_input.kR[1], so3_command.kR[1] * 50); 57 | SATURATE_CAST(so3_cmd_input.kR[2], so3_command.kR[2] * 50); 58 | 59 | SATURATE_CAST(so3_cmd_input.kOm[0], so3_command.kOm[0] * 100); 60 | SATURATE_CAST(so3_cmd_input.kOm[1], so3_command.kOm[1] * 100); 61 | SATURATE_CAST(so3_cmd_input.kOm[2], so3_command.kOm[2] * 100); 62 | 63 | SATURATE_CAST(so3_cmd_input.cur_yaw, so3_command.aux.current_yaw * 1e4f); 64 | 65 | SATURATE_CAST(so3_cmd_input.kf_correction, so3_command.aux.kf_correction * 1e11f); 66 | SATURATE_CAST(so3_cmd_input.angle_corrections[0], so3_command.aux.angle_corrections[0] * 2500); 67 | SATURATE_CAST(so3_cmd_input.angle_corrections[1], so3_command.aux.angle_corrections[1] * 2500); 68 | 69 | so3_cmd_input.enable_motors = so3_command.aux.enable_motors; 70 | so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; 71 | 72 | so3_cmd_input.seq = so3_command.header.seq % 255; 73 | 74 | output.resize(sizeof(so3_cmd_input)); 75 | memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); 76 | } 77 | 78 | void encodeTRPYCommand(const kr_mav_msgs::TRPYCommand &trpy_command, std::vector &output) 79 | { 80 | struct TRPY_CMD trpy_cmd_input; 81 | 82 | SATURATE_CAST(trpy_cmd_input.thrust, trpy_command.thrust * 1e4f); 83 | SATURATE_CAST(trpy_cmd_input.roll, trpy_command.roll * 1e4f); 84 | SATURATE_CAST(trpy_cmd_input.pitch, trpy_command.pitch * 1e4f); 85 | SATURATE_CAST(trpy_cmd_input.yaw, trpy_command.yaw * 1e4f); 86 | SATURATE_CAST(trpy_cmd_input.current_yaw, trpy_command.aux.current_yaw * 1e4f); 87 | 88 | trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; 89 | trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; 90 | 91 | output.resize(sizeof(trpy_cmd_input)); 92 | memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); 93 | } 94 | 95 | void encodePWMCommand(const kr_mav_msgs::PWMCommand &pwm_command, std::vector &output) 96 | { 97 | struct PWM_CMD_INPUT pwm_cmd_input; 98 | 99 | SATURATE_CAST(pwm_cmd_input.pwm[0], pwm_command.pwm[0] * 255); 100 | SATURATE_CAST(pwm_cmd_input.pwm[1], pwm_command.pwm[1] * 255); 101 | 102 | output.resize(sizeof(pwm_cmd_input)); 103 | memcpy(&output[0], &pwm_cmd_input, sizeof(pwm_cmd_input)); 104 | } 105 | } // namespace kr_mav_msgs 106 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/quad_decode_msg_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class QuadDecodeMsg : public nodelet::Nodelet 8 | { 9 | public: 10 | void onInit(void); 11 | 12 | private: 13 | void serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg); 14 | ros::Publisher output_data_pub_, imu_output_pub_, status_pub_; 15 | ros::Subscriber serial_sub_; 16 | }; 17 | 18 | void QuadDecodeMsg::serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg) 19 | { 20 | if(msg->type == kr_mav_msgs::Serial::OUTPUT_DATA) 21 | { 22 | kr_mav_msgs::OutputData::Ptr output_msg(new kr_mav_msgs::OutputData); 23 | sensor_msgs::Imu::Ptr imu_msg(new sensor_msgs::Imu); 24 | 25 | if(kr_mav_msgs::decodeOutputData(msg->data, *output_msg)) 26 | { 27 | output_msg->header.stamp = msg->header.stamp; 28 | output_msg->header.frame_id = "/quadrotor"; 29 | output_data_pub_.publish(output_msg); 30 | 31 | imu_msg->header = output_msg->header; 32 | imu_msg->orientation = output_msg->orientation; 33 | imu_msg->angular_velocity = output_msg->angular_velocity; 34 | imu_msg->linear_acceleration = output_msg->linear_acceleration; 35 | imu_output_pub_.publish(imu_msg); 36 | } 37 | } 38 | else if(msg->type == kr_mav_msgs::Serial::STATUS_DATA) 39 | { 40 | kr_mav_msgs::StatusData::Ptr status_msg(new kr_mav_msgs::StatusData); 41 | if(kr_mav_msgs::decodeStatusData(msg->data, *status_msg)) 42 | { 43 | status_msg->header.stamp = msg->header.stamp; 44 | status_msg->header.frame_id = "/quadrotor"; 45 | status_pub_.publish(status_msg); 46 | } 47 | } 48 | } 49 | 50 | void QuadDecodeMsg::onInit(void) 51 | { 52 | ros::NodeHandle priv_nh(getPrivateNodeHandle()); 53 | 54 | output_data_pub_ = priv_nh.advertise("output_data", 10); 55 | imu_output_pub_ = priv_nh.advertise("imu", 10); 56 | status_pub_ = priv_nh.advertise("status", 10); 57 | 58 | serial_sub_ = 59 | priv_nh.subscribe("serial", 10, &QuadDecodeMsg::serial_callback, this, ros::TransportHints().tcpNoDelay()); 60 | } 61 | 62 | #include 63 | PLUGINLIB_EXPORT_CLASS(QuadDecodeMsg, nodelet::Nodelet); 64 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/quad_encode_msg.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | static ros::Publisher serial_msg_pub; 9 | static int channel; 10 | 11 | static void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) 12 | { 13 | kr_mav_msgs::Serial serial_msg; 14 | serial_msg.header.seq = msg->header.seq; 15 | serial_msg.channel = channel; 16 | serial_msg.type = kr_mav_msgs::Serial::SO3_CMD; 17 | 18 | kr_mav_msgs::encodeSO3Command(*msg, serial_msg.data); 19 | 20 | serial_msg.header.stamp = ros::Time::now(); 21 | serial_msg_pub.publish(serial_msg); 22 | } 23 | 24 | static void trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) 25 | { 26 | kr_mav_msgs::Serial serial_msg; 27 | serial_msg.header.seq = msg->header.seq; 28 | serial_msg.channel = channel; 29 | serial_msg.type = kr_mav_msgs::Serial::TRPY_CMD; 30 | 31 | kr_mav_msgs::encodeTRPYCommand(*msg, serial_msg.data); 32 | 33 | serial_msg.header.stamp = ros::Time::now(); 34 | serial_msg_pub.publish(serial_msg); 35 | } 36 | 37 | static void pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg) 38 | { 39 | kr_mav_msgs::Serial serial_msg; 40 | serial_msg.header.seq = msg->header.seq; 41 | serial_msg.channel = channel; 42 | serial_msg.type = kr_mav_msgs::Serial::PWM_CMD; 43 | 44 | kr_mav_msgs::encodePWMCommand(*msg, serial_msg.data); 45 | 46 | serial_msg.header.stamp = ros::Time::now(); 47 | serial_msg_pub.publish(serial_msg); 48 | } 49 | 50 | int main(int argc, char **argv) 51 | { 52 | ros::init(argc, argv, "quad_encode_msg"); 53 | 54 | ros::NodeHandle n("~"); 55 | 56 | n.param("channel", channel, 0); 57 | 58 | ros::Subscriber so3_cmd_sub = n.subscribe("so3_cmd", 10, &so3_cmd_callback, ros::TransportHints().tcpNoDelay()); 59 | 60 | ros::Subscriber trpy_cmd_sub = n.subscribe("trpy_cmd", 10, &so3_cmd_callback, ros::TransportHints().tcpNoDelay()); 61 | 62 | ros::Subscriber pwm_cmd_sub = n.subscribe("pwm_cmd", 10, &pwm_cmd_callback, ros::TransportHints().tcpNoDelay()); 63 | 64 | serial_msg_pub = n.advertise("serial_msg", 10); 65 | 66 | ros::spin(); 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/quad_encode_msg_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class QuadEncodeMsg : public nodelet::Nodelet 10 | { 11 | public: 12 | void onInit(void); 13 | 14 | private: 15 | void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); 16 | void trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg); 17 | void pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg); 18 | ros::Publisher serial_msg_pub_; 19 | ros::Subscriber so3_cmd_sub_; 20 | ros::Subscriber trpy_cmd_sub_; 21 | ros::Subscriber pwm_cmd_sub_; 22 | int channel_; 23 | }; 24 | 25 | void QuadEncodeMsg::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) 26 | { 27 | kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); 28 | serial_msg->header.seq = msg->header.seq; 29 | serial_msg->channel = channel_; 30 | serial_msg->type = kr_mav_msgs::Serial::SO3_CMD; 31 | 32 | kr_mav_msgs::encodeSO3Command(*msg, serial_msg->data); 33 | 34 | serial_msg->header.stamp = ros::Time::now(); 35 | serial_msg_pub_.publish(serial_msg); 36 | } 37 | 38 | void QuadEncodeMsg::trpy_cmd_callback(const kr_mav_msgs::TRPYCommand::ConstPtr &msg) 39 | { 40 | kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); 41 | serial_msg->header.seq = msg->header.seq; 42 | serial_msg->channel = channel_; 43 | serial_msg->type = kr_mav_msgs::Serial::TRPY_CMD; 44 | 45 | kr_mav_msgs::encodeTRPYCommand(*msg, serial_msg->data); 46 | 47 | serial_msg->header.stamp = ros::Time::now(); 48 | serial_msg_pub_.publish(serial_msg); 49 | } 50 | 51 | void QuadEncodeMsg::pwm_cmd_callback(const kr_mav_msgs::PWMCommand::ConstPtr &msg) 52 | { 53 | kr_mav_msgs::Serial::Ptr serial_msg(new kr_mav_msgs::Serial); 54 | serial_msg->header.seq = msg->header.seq; 55 | serial_msg->channel = channel_; 56 | serial_msg->type = kr_mav_msgs::Serial::PWM_CMD; 57 | kr_mav_msgs::encodePWMCommand(*msg, serial_msg->data); 58 | 59 | serial_msg->header.stamp = ros::Time::now(); 60 | serial_msg_pub_.publish(serial_msg); 61 | } 62 | 63 | void QuadEncodeMsg::onInit(void) 64 | { 65 | ros::NodeHandle priv_nh(getPrivateNodeHandle()); 66 | 67 | priv_nh.param("channel", channel_, 0); 68 | 69 | serial_msg_pub_ = priv_nh.advertise("serial_msg", 10); 70 | 71 | so3_cmd_sub_ = 72 | priv_nh.subscribe("so3_cmd", 10, &QuadEncodeMsg::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); 73 | trpy_cmd_sub_ = 74 | priv_nh.subscribe("trpy_cmd", 10, &QuadEncodeMsg::trpy_cmd_callback, this, ros::TransportHints().tcpNoDelay()); 75 | pwm_cmd_sub_ = 76 | priv_nh.subscribe("pwm_cmd", 10, &QuadEncodeMsg::pwm_cmd_callback, this, ros::TransportHints().tcpNoDelay()); 77 | } 78 | 79 | #include 80 | PLUGINLIB_EXPORT_CLASS(QuadEncodeMsg, nodelet::Nodelet); 81 | -------------------------------------------------------------------------------- /interfaces/kr_serial_interface/src/quad_serial_comm_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class QuadSerialComm : public nodelet::Nodelet 8 | { 9 | public: 10 | void onInit(void); 11 | ~QuadSerialComm(); 12 | 13 | private: 14 | void serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg); 15 | void output_data_callback(kr_mav_msgs::Serial &msg); 16 | void serial_read_callback(const unsigned char *data, size_t count); 17 | 18 | ASIOSerialDevice sd_; 19 | ros::Publisher output_data_pub_; 20 | ros::Subscriber serial_sub_; 21 | }; 22 | 23 | void QuadSerialComm::serial_callback(const kr_mav_msgs::Serial::ConstPtr &msg) 24 | { 25 | std::vector serial_msg; 26 | 27 | encode_serial_msg(*msg, serial_msg); 28 | 29 | sd_.Write(serial_msg); 30 | } 31 | 32 | void QuadSerialComm::output_data_callback(kr_mav_msgs::Serial &msg) 33 | { 34 | msg.header.stamp = ros::Time::now(); 35 | output_data_pub_.publish(msg); 36 | } 37 | 38 | void QuadSerialComm::serial_read_callback(const unsigned char *data, size_t count) 39 | { 40 | process_serial_data(data, count, boost::bind(&QuadSerialComm::output_data_callback, this, _1)); 41 | } 42 | 43 | void QuadSerialComm::onInit(void) 44 | { 45 | ros::NodeHandle n(getPrivateNodeHandle()); 46 | 47 | std::string device; 48 | n.param("device", device, std::string("/dev/ttyUSB0")); 49 | 50 | int baud_rate; 51 | n.param("baud_rate", baud_rate, 57600); 52 | 53 | sd_.Open(device, baud_rate); 54 | 55 | output_data_pub_ = n.advertise("from_robot", 10); 56 | 57 | serial_sub_ = n.subscribe("to_robot", 10, &QuadSerialComm::serial_callback, this, ros::TransportHints().tcpNoDelay()); 58 | 59 | sd_.SetReadCallback(boost::bind(&QuadSerialComm::serial_read_callback, this, _1, _2)); 60 | sd_.Start(); 61 | } 62 | 63 | QuadSerialComm::~QuadSerialComm() 64 | { 65 | sd_.Close(); 66 | sd_.Stop(); 67 | } 68 | 69 | #include 70 | PLUGINLIB_EXPORT_CLASS(QuadSerialComm, nodelet::Nodelet); 71 | -------------------------------------------------------------------------------- /kr_mav_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mav_controllers) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package(catkin REQUIRED COMPONENTS 15 | dynamic_reconfigure 16 | geometry_msgs 17 | kr_mav_msgs 18 | nav_msgs 19 | nodelet 20 | roscpp 21 | std_msgs 22 | tf 23 | ) 24 | find_package(Eigen3 REQUIRED NO_MODULE) 25 | 26 | generate_dynamic_reconfigure_options(cfg/SO3.cfg) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS 30 | LIBRARIES 31 | CATKIN_DEPENDS 32 | dynamic_reconfigure 33 | geometry_msgs 34 | kr_mav_msgs 35 | nav_msgs 36 | nodelet 37 | roscpp 38 | std_msgs 39 | tf 40 | DEPENDS 41 | EIGEN3 42 | ) 43 | 44 | add_library(kr_mav_so3_control src/SO3Control.cpp src/so3_control_nodelet.cpp src/so3_trpy_control.cpp) 45 | target_include_directories(kr_mav_so3_control PUBLIC include ${catkin_INCLUDE_DIRS}) 46 | target_link_libraries(kr_mav_so3_control PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 47 | add_dependencies(kr_mav_so3_control ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 48 | 49 | # TODO: PID control has not been updated for new messages add_library(kr_mav_pid_control src/PIDControl.cpp 50 | # src/pid_control_nodelet.cpp) add_dependencies(kr_mav_pid_control ${catkin_EXPORTED_TARGETS}) 51 | # target_link_libraries(kr_mav_pid_control ${catkin_LIBRARIES}) 52 | 53 | install( 54 | TARGETS kr_mav_so3_control 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 58 | ) 59 | 60 | # install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 61 | install(FILES nodelet_plugin.xml 62 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 63 | ) 64 | 65 | if(CATKIN_ENABLE_TESTING) 66 | find_package(catkin REQUIRED COMPONENTS 67 | roscpp 68 | std_msgs 69 | nav_msgs 70 | kr_mav_msgs 71 | rostest 72 | ) 73 | 74 | include_directories(include ${catkin_INCLUDE_DIRS}) 75 | include_directories(${GTEST_INCLUDE_DIRS}) 76 | 77 | add_executable(so3_control_nodelet_test test/so3_control_nodelet_test.cpp) 78 | # add_rostest_gtest(so3_control_nodelet_test test/so3_control_nodelet.test test/so3_control_nodelet_test.cpp) 79 | target_link_libraries(so3_control_nodelet_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) 80 | 81 | add_rostest(test/so3_control_nodelet.test) 82 | 83 | endif() 84 | -------------------------------------------------------------------------------- /kr_mav_controllers/cfg/SO3.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | PACKAGE = "kr_mav_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | tg = gen.add_group("Translation Gains") 9 | # World Position Gains 10 | tg.add("kp_x", double_t, 1, "World x position gain", 7.4, 0.0, 20.0) 11 | tg.add("kp_y", double_t, 1, "World y position gain", 7.4, 0.0, 20.0) 12 | tg.add("kp_z", double_t, 1, "World z position gain", 10.4, 0.0, 20.0) 13 | # World Derivative Gai 14 | tg.add("kd_x", double_t, 1, "World x derivative gain", 4.8, 0.0, 20.0) 15 | tg.add("kd_y", double_t, 1, "World y derivative gain", 4.8, 0.0, 20.0) 16 | tg.add("kd_z", double_t, 1, "World z derivative gain", 6.0, 0.0, 20.0) 17 | 18 | ig = gen.add_group("Integral Gains") 19 | # World Integral Gains 20 | ig.add("ki_x", double_t, 2, "World x integral gain", 0.0, 0.0, 0.2) 21 | ig.add("ki_y", double_t, 2, "World y integral gain", 0.0, 0.0, 0.2) 22 | ig.add("ki_z", double_t, 2, "World z integral gain", 0.0, 0.0, 0.2) 23 | # Body Integral Gains 24 | ig.add("kib_x", double_t, 2, "Body x integral gain", 0.0, 0.0, 0.2) 25 | ig.add("kib_y", double_t, 2, "Body y integral gain", 0.0, 0.0, 0.2) 26 | ig.add("kib_z", double_t, 2, "Body z integral gain", 0.0, 0.0, 0.2) 27 | 28 | ag = gen.add_group("Attitude Gains") 29 | # Rotation Gains 30 | ag.add("rot_x", double_t, 4, "Rotation x gain", 1.5, 0.0, 3.00) 31 | ag.add("rot_y", double_t, 4, "Rotation y gain", 1.5, 0.0, 3.00) 32 | ag.add("rot_z", double_t, 4, "Rotation z gain", 1.0, 0.0, 3.00) 33 | # Angular Gains 34 | ag.add("ang_x", double_t, 4, "Angular x gain", 0.13, 0.0, 1.00) 35 | ag.add("ang_y", double_t, 4, "Angular y gain", 0.13, 0.0, 1.00) 36 | ag.add("ang_z", double_t, 4, "Angular z gain", 0.10, 0.0, 1.00) 37 | 38 | # Low level corrections 39 | corr = gen.add_group("Corrections") 40 | # TODO: Determine limits 41 | corr.add("kf_correction", double_t, 8, "kf", 0.0, 0.0, 0.0) 42 | corr.add("roll_correction", double_t, 8, "roll", 0.0, 0.0, 0.0) 43 | corr.add("pitch_correction", double_t, 8, "pitch", 0.0, 0.0, 0.0) 44 | 45 | # Misc 46 | misc = gen.add_group("Maxes and limits") 47 | misc.add("max_pos_int", double_t, 16, "World max integral", 0.0, 0.0, 4.00) 48 | misc.add("max_pos_int_b", double_t, 16, "Body max integral", 0.0, 0.0, 4.00) 49 | misc.add("max_tilt_angle", double_t, 16, "Max tilt angle", 3.14, 0.0, 3.14) 50 | 51 | exit(gen.generate(PACKAGE, "kr_mav_controllers", "SO3")) 52 | -------------------------------------------------------------------------------- /kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PID_CONTROL_H 2 | #define PID_CONTROL_H 3 | 4 | #include 5 | 6 | class PIDControl 7 | { 8 | public: 9 | PIDControl(); 10 | 11 | void setMass(const float mass); 12 | void setGravity(const float g); 13 | void setPosition(const Eigen::Vector3f &position); 14 | void setVelocity(const Eigen::Vector3f &velocity); 15 | void setYaw(const float current_yaw); 16 | void setMaxIntegral(const float max_integral); 17 | void resetIntegrals(void); 18 | 19 | void calculateControl(const Eigen::Vector3f &des_pos, const Eigen::Vector3f &des_vel, const Eigen::Vector3f &des_acc, 20 | const float des_yaw, const Eigen::Vector3f &kx, const Eigen::Vector3f &kv, 21 | const Eigen::Vector3f &ki, const float ki_yaw); 22 | 23 | const Eigen::Vector4f &getControls(void); 24 | 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 26 | 27 | private: 28 | // Inputs for the controller 29 | float mass_; 30 | float g_; 31 | Eigen::Vector3f pos_; 32 | Eigen::Vector3f vel_; 33 | float current_yaw_; 34 | Eigen::Vector3f pos_int_; 35 | float yaw_int_; 36 | float max_pos_int_; 37 | 38 | // Outputs of the controller 39 | Eigen::Vector4f trpy_; 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /kr_mav_controllers/include/kr_mav_controllers/SO3Control.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_CONTROL_H 2 | #define SO3_CONTROL_H 3 | 4 | #include 5 | 6 | class SO3Control 7 | { 8 | public: 9 | SO3Control(); 10 | 11 | void setMass(const float mass); 12 | void setGravity(const float g); 13 | void setPosition(const Eigen::Vector3f &position); 14 | void setVelocity(const Eigen::Vector3f &velocity); 15 | void setMaxIntegral(const float max_integral); 16 | void setMaxIntegralBody(const float max_integral_b); 17 | void setCurrentOrientation(const Eigen::Quaternionf ¤t_orientation); 18 | void resetIntegrals(); 19 | void setMaxTiltAngle(const float max_tilt_angle); 20 | 21 | void calculateControl(const Eigen::Vector3f &des_pos, const Eigen::Vector3f &des_vel, const Eigen::Vector3f &des_acc, 22 | const Eigen::Vector3f &des_jerk, const float des_yaw, const float des_yaw_dot, 23 | const Eigen::Vector3f &kx, const Eigen::Vector3f &kv, const Eigen::Vector3f &ki, 24 | const Eigen::Vector3f &ki_b); 25 | 26 | const Eigen::Vector3f &getComputedForce(); 27 | const Eigen::Quaternionf &getComputedOrientation(); 28 | const Eigen::Vector3f &getComputedAngularVelocity(); 29 | 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | 32 | private: 33 | // Inputs for the controller 34 | float mass_; 35 | float g_; 36 | Eigen::Vector3f pos_; 37 | Eigen::Vector3f vel_; 38 | float max_pos_int_; 39 | float max_pos_int_b_; 40 | Eigen::Quaternionf current_orientation_; 41 | float cos_max_tilt_angle_; 42 | 43 | // Outputs of the controller 44 | Eigen::Vector3f force_; 45 | Eigen::Quaternionf orientation_; 46 | Eigen::Vector3f angular_velocity_; 47 | Eigen::Vector3f pos_int_; 48 | Eigen::Vector3f pos_int_b_; 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /kr_mav_controllers/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kr_mav_so3_controllers 5 | 6 | 7 | 8 | 9 | 10 | kr_mav_so3_controllers with thrust, roll, pitch, yaw output 11 | 12 | 13 | 14 | 15 | 24 | -------------------------------------------------------------------------------- /kr_mav_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_controllers 3 | 1.0.0 4 | kr_mav_controllers 5 | Kartik Mohta 6 | Michael Watterson 7 | 8 | BSD 9 | 10 | Kartik Mohta 11 | 12 | catkin 13 | 14 | dynamic_reconfigure 15 | geometry_msgs 16 | kr_mav_msgs 17 | nav_msgs 18 | nodelet 19 | roscpp 20 | std_msgs 21 | tf 22 | 23 | rostest 24 | gtest 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /kr_mav_controllers/src/PIDControl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | PIDControl::PIDControl() : mass_(0.5), g_(9.81), yaw_int_(0.0), max_pos_int_(0.5) {} 5 | 6 | void PIDControl::setMass(const float mass) 7 | { 8 | mass_ = mass; 9 | } 10 | 11 | void PIDControl::setGravity(const float g) 12 | { 13 | g_ = g; 14 | } 15 | 16 | void PIDControl::setPosition(const Eigen::Vector3f &position) 17 | { 18 | pos_ = position; 19 | } 20 | 21 | void PIDControl::setVelocity(const Eigen::Vector3f &velocity) 22 | { 23 | vel_ = velocity; 24 | } 25 | 26 | void PIDControl::setYaw(const float current_yaw) 27 | { 28 | current_yaw_ = current_yaw; 29 | } 30 | 31 | void PIDControl::setMaxIntegral(const float max_integral) 32 | { 33 | max_pos_int_ = max_integral; 34 | } 35 | 36 | void PIDControl::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::Vector3f &des_vel, 37 | const Eigen::Vector3f &des_acc, const float des_yaw, const Eigen::Vector3f &kx, 38 | const Eigen::Vector3f &kv, const Eigen::Vector3f &ki, const float ki_yaw) 39 | { 40 | Eigen::Vector3f e_pos = (des_pos - pos_); 41 | Eigen::Vector3f e_vel = (des_vel - vel_); 42 | for(int i = 0; i < 3; i++) 43 | { 44 | if(kx(i) != 0) 45 | pos_int_(i) += ki(i) * e_pos(i); 46 | 47 | // Limit integral term 48 | if(pos_int_(i) > max_pos_int_) 49 | pos_int_(i) = max_pos_int_; 50 | else if(pos_int_(i) < -max_pos_int_) 51 | pos_int_(i) = -max_pos_int_; 52 | } 53 | 54 | // std::cout << pos_int_.transpose() << std::endl; 55 | 56 | Eigen::Vector3f force_des = kx.asDiagonal() * e_pos + pos_int_ + kv.asDiagonal() * e_vel + mass_ * des_acc + 57 | mass_ * g_ * Eigen::Vector3f(0, 0, 1); 58 | 59 | float roll_des = (force_des(0) * sin(current_yaw_) - force_des(1) * cos(current_yaw_)) / force_des(2); 60 | float pitch_des = (force_des(0) * cos(current_yaw_) + force_des(1) * sin(current_yaw_)) / force_des(2); 61 | 62 | float e_yaw = (des_yaw - current_yaw_); 63 | if(e_yaw > M_PI) 64 | e_yaw -= 2 * M_PI; 65 | else if(e_yaw < -M_PI) 66 | e_yaw += 2 * M_PI; 67 | 68 | yaw_int_ += ki_yaw * e_yaw; 69 | if(yaw_int_ > M_PI) 70 | yaw_int_ = M_PI; 71 | else if(yaw_int_ < -M_PI) 72 | yaw_int_ = -M_PI; 73 | 74 | float yaw_cmd = des_yaw + yaw_int_; 75 | if(yaw_cmd > M_PI) 76 | yaw_cmd -= 2 * M_PI; 77 | else if(yaw_cmd < -M_PI) 78 | yaw_cmd += 2 * M_PI; 79 | 80 | trpy_(0) = force_des(2); 81 | trpy_(1) = roll_des; 82 | trpy_(2) = pitch_des; 83 | trpy_(3) = yaw_cmd; 84 | } 85 | 86 | const Eigen::Vector4f &PIDControl::getControls(void) 87 | { 88 | return trpy_; 89 | } 90 | 91 | void PIDControl::resetIntegrals(void) 92 | { 93 | pos_int_ = Eigen::Vector3f::Zero(); 94 | yaw_int_ = 0; 95 | } 96 | -------------------------------------------------------------------------------- /kr_mav_controllers/src/SO3Control.cpp: -------------------------------------------------------------------------------- 1 | #include "kr_mav_controllers/SO3Control.h" 2 | 3 | #include 4 | #include 5 | 6 | SO3Control::SO3Control() 7 | : mass_(0.5), 8 | g_(9.81), 9 | max_pos_int_(0.5), 10 | max_pos_int_b_(0.5), 11 | current_orientation_(Eigen::Quaternionf::Identity()), 12 | cos_max_tilt_angle_(-1.0) 13 | { 14 | } 15 | 16 | void SO3Control::setMass(const float mass) 17 | { 18 | mass_ = mass; 19 | } 20 | 21 | void SO3Control::setGravity(const float g) 22 | { 23 | g_ = g; 24 | } 25 | 26 | void SO3Control::setPosition(const Eigen::Vector3f &position) 27 | { 28 | pos_ = position; 29 | } 30 | 31 | void SO3Control::setVelocity(const Eigen::Vector3f &velocity) 32 | { 33 | vel_ = velocity; 34 | } 35 | 36 | void SO3Control::setMaxIntegral(const float max_integral) 37 | { 38 | max_pos_int_ = max_integral; 39 | } 40 | 41 | void SO3Control::setMaxIntegralBody(const float max_integral_b) 42 | { 43 | max_pos_int_b_ = max_integral_b; 44 | } 45 | 46 | void SO3Control::setCurrentOrientation(const Eigen::Quaternionf ¤t_orientation) 47 | { 48 | current_orientation_ = current_orientation; 49 | } 50 | 51 | void SO3Control::setMaxTiltAngle(const float max_tilt_angle) 52 | { 53 | if(max_tilt_angle > 0.0f && max_tilt_angle <= static_cast(M_PI)) 54 | cos_max_tilt_angle_ = std::cos(max_tilt_angle); 55 | } 56 | 57 | void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::Vector3f &des_vel, 58 | const Eigen::Vector3f &des_acc, const Eigen::Vector3f &des_jerk, const float des_yaw, 59 | const float des_yaw_dot, const Eigen::Vector3f &kx, const Eigen::Vector3f &kv, 60 | const Eigen::Vector3f &ki, const Eigen::Vector3f &ki_b) 61 | { 62 | const Eigen::Vector3f e_pos = des_pos - pos_; 63 | const Eigen::Vector3f e_vel = des_vel - vel_; 64 | 65 | for(int i = 0; i < 3; i++) 66 | { 67 | if(kx(i) != 0) 68 | pos_int_(i) += ki(i) * e_pos(i); 69 | 70 | // Limit integral term 71 | if(pos_int_(i) > max_pos_int_) 72 | pos_int_(i) = max_pos_int_; 73 | else if(pos_int_(i) < -max_pos_int_) 74 | pos_int_(i) = -max_pos_int_; 75 | } 76 | // ROS_DEBUG_THROTTLE(2, "Integrated world disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", pos_int_(0), 77 | // pos_int_(1), pos_int_(2)); 78 | 79 | Eigen::Quaternionf q(current_orientation_); 80 | const Eigen::Vector3f e_pos_b = q.inverse() * e_pos; 81 | for(int i = 0; i < 3; i++) 82 | { 83 | if(kx(i) != 0) 84 | pos_int_b_(i) += ki_b(i) * e_pos_b(i); 85 | 86 | // Limit integral term in the body 87 | if(pos_int_b_(i) > max_pos_int_b_) 88 | pos_int_b_(i) = max_pos_int_b_; 89 | else if(pos_int_b_(i) < -max_pos_int_b_) 90 | pos_int_b_(i) = -max_pos_int_b_; 91 | } 92 | // ROS_DEBUG_THROTTLE(2, "Integrated body disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", 93 | // pos_int_b_(0), pos_int_b_(1), pos_int_b_(2)); 94 | 95 | const Eigen::Vector3f acc_grav = g_ * Eigen::Vector3f::UnitZ(); 96 | const Eigen::Vector3f acc_control = kx.asDiagonal() * e_pos + kv.asDiagonal() * e_vel + pos_int_ + des_acc; 97 | Eigen::Vector3f acc_total = acc_control + acc_grav; 98 | 99 | // Check and limit tilt angle 100 | float lambda = 1.0f; 101 | if(Eigen::Vector3f::UnitZ().dot(acc_total.normalized()) < cos_max_tilt_angle_) 102 | { 103 | const float x = acc_control.x(), y = acc_control.y(), z = acc_control.z(); 104 | const float cot_max_tilt_angle = cos_max_tilt_angle_ / std::sqrt(1 - cos_max_tilt_angle_ * cos_max_tilt_angle_); 105 | lambda = -g_ / (z - std::sqrt(x * x + y * y) * cot_max_tilt_angle); 106 | if(lambda > 0 && lambda <= 1) 107 | acc_total = lambda * acc_control + acc_grav; 108 | } 109 | 110 | force_.noalias() = mass_ * acc_total; 111 | 112 | // std::cout << "Force: " << force_.transpose() << std::endl; 113 | 114 | Eigen::Vector3f b1c, b2c, b3c; 115 | const Eigen::Vector3f b2d(-std::sin(des_yaw), std::cos(des_yaw), 0); 116 | 117 | if(force_.norm() > 1e-6f) 118 | b3c.noalias() = force_.normalized(); 119 | else 120 | b3c.noalias() = Eigen::Vector3f::UnitZ(); 121 | 122 | b1c.noalias() = b2d.cross(b3c).normalized(); 123 | b2c.noalias() = b3c.cross(b1c).normalized(); 124 | 125 | const Eigen::Vector3f force_dot = 126 | mass_ * lambda * (kx.asDiagonal() * e_vel + des_jerk); // Ignoring kv*e_acc and ki*e_pos terms 127 | const Eigen::Vector3f b3c_dot = b3c.cross(force_dot / force_.norm()).cross(b3c); 128 | const Eigen::Vector3f b2d_dot(-std::cos(des_yaw) * des_yaw_dot, -std::sin(des_yaw) * des_yaw_dot, 0); 129 | const Eigen::Vector3f b1c_dot = 130 | b1c.cross(((b2d_dot.cross(b3c) + b2d.cross(b3c_dot)) / (b2d.cross(b3c)).norm()).cross(b1c)); 131 | const Eigen::Vector3f b2c_dot = b3c_dot.cross(b1c) + b3c.cross(b1c_dot); 132 | 133 | Eigen::Matrix3f R; 134 | R << b1c, b2c, b3c; 135 | orientation_ = Eigen::Quaternionf(R); 136 | 137 | Eigen::Matrix3f R_dot; 138 | R_dot << b1c_dot, b2c_dot, b3c_dot; 139 | 140 | const Eigen::Matrix3f omega_hat = R.transpose() * R_dot; 141 | angular_velocity_ = Eigen::Vector3f(omega_hat(2, 1), omega_hat(0, 2), omega_hat(1, 0)); 142 | } 143 | 144 | const Eigen::Vector3f &SO3Control::getComputedForce() 145 | { 146 | return force_; 147 | } 148 | 149 | const Eigen::Quaternionf &SO3Control::getComputedOrientation() 150 | { 151 | return orientation_; 152 | } 153 | 154 | const Eigen::Vector3f &SO3Control::getComputedAngularVelocity() 155 | { 156 | return angular_velocity_; 157 | } 158 | 159 | void SO3Control::resetIntegrals() 160 | { 161 | pos_int_ = Eigen::Vector3f::Zero(); 162 | pos_int_b_ = Eigen::Vector3f::Zero(); 163 | } 164 | -------------------------------------------------------------------------------- /kr_mav_controllers/src/pid_control_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) 13 | 14 | class PIDControlNodelet : public nodelet::Nodelet 15 | { 16 | public: 17 | PIDControlNodelet() 18 | : position_cmd_updated_(false), 19 | position_cmd_init_(false), 20 | des_yaw_(0), 21 | current_yaw_(0), 22 | enable_motors_(false), 23 | use_external_yaw_(false) 24 | { 25 | } 26 | 27 | void onInit(void); 28 | 29 | private: 30 | void publishTRPYCommand(void); 31 | void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); 32 | void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); 33 | void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); 34 | 35 | PIDControl controller_; 36 | ros::Publisher trpy_command_pub_; 37 | ros::Subscriber odom_sub_; 38 | ros::Subscriber position_cmd_sub_; 39 | ros::Subscriber enable_motors_sub_; 40 | 41 | bool position_cmd_updated_, position_cmd_init_; 42 | std::string frame_id_; 43 | 44 | Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_; 45 | float des_yaw_; 46 | float current_yaw_; 47 | float ki_yaw_; 48 | float max_roll_pitch_; 49 | bool enable_motors_; 50 | bool use_external_yaw_; 51 | }; 52 | 53 | void PIDControlNodelet::publishTRPYCommand(void) 54 | { 55 | float ki_yaw = 0; 56 | Eigen::Vector3f ki = Eigen::Vector3f::Zero(); 57 | // Only enable integral terms when motors are on 58 | if(enable_motors_) 59 | { 60 | ki_yaw = ki_yaw_; 61 | ki = ki_; 62 | } 63 | controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw); 64 | 65 | const Eigen::Vector4f &trpy = controller_.getControls(); 66 | 67 | kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand); 68 | trpy_command->header.stamp = ros::Time::now(); 69 | trpy_command->header.frame_id = frame_id_; 70 | if(enable_motors_) 71 | { 72 | trpy_command->thrust = trpy(0); 73 | trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_); 74 | trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_); 75 | trpy_command->yaw = trpy(3); 76 | } 77 | trpy_command->aux.current_yaw = current_yaw_; 78 | trpy_command->aux.enable_motors = enable_motors_; 79 | trpy_command->aux.use_external_yaw = use_external_yaw_; 80 | trpy_command_pub_.publish(trpy_command); 81 | } 82 | 83 | void PIDControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) 84 | { 85 | des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); 86 | des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); 87 | des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); 88 | kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]); 89 | kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]); 90 | 91 | des_yaw_ = cmd->yaw; 92 | position_cmd_updated_ = true; 93 | // position_cmd_init_ = true; 94 | 95 | publishTRPYCommand(); 96 | } 97 | 98 | void PIDControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) 99 | { 100 | const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); 101 | const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); 102 | 103 | current_yaw_ = tf::getYaw(odom->pose.pose.orientation); 104 | 105 | controller_.setPosition(position); 106 | controller_.setVelocity(velocity); 107 | controller_.setYaw(current_yaw_); 108 | 109 | if(position_cmd_init_) 110 | { 111 | // We set position_cmd_updated_ = false and expect that the 112 | // position_cmd_callback would set it to true since typically a position_cmd 113 | // message would follow an odom message. If not, the position_cmd_callback 114 | // hasn't been called and we publish the so3 command ourselves 115 | // TODO: Fallback to hover if position_cmd hasn't been received for some time 116 | if(!position_cmd_updated_) 117 | publishTRPYCommand(); 118 | position_cmd_updated_ = false; 119 | } 120 | } 121 | 122 | void PIDControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) 123 | { 124 | if(msg->data) 125 | ROS_INFO("Enabling motors"); 126 | else 127 | ROS_INFO("Disabling motors"); 128 | 129 | enable_motors_ = msg->data; 130 | 131 | // Reset integrals when toggling motor state 132 | controller_.resetIntegrals(); 133 | } 134 | 135 | void PIDControlNodelet::onInit(void) 136 | { 137 | ros::NodeHandle n(getPrivateNodeHandle()); 138 | 139 | std::string quadrotor_name; 140 | n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); 141 | frame_id_ = "/" + quadrotor_name; 142 | 143 | double mass; 144 | n.param("mass", mass, 0.235); 145 | controller_.setMass(mass); 146 | controller_.setMaxIntegral(mass * 3); 147 | 148 | n.param("use_external_yaw", use_external_yaw_, true); 149 | 150 | double max_roll_pitch; 151 | n.param("max_roll_pitch", max_roll_pitch, 30.0); 152 | max_roll_pitch_ = max_roll_pitch; 153 | 154 | double ki_x, ki_y, ki_z, ki_yaw; 155 | n.param("gains/ki/x", ki_x, 0.0); 156 | n.param("gains/ki/y", ki_y, 0.0); 157 | n.param("gains/ki/z", ki_z, 0.0); 158 | n.param("gains/ki/yaw", ki_yaw, 0.01); 159 | ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z; 160 | ki_yaw_ = ki_yaw; 161 | 162 | trpy_command_pub_ = n.advertise("trpy_cmd", 10); 163 | 164 | odom_sub_ = n.subscribe("odom", 10, &PIDControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); 165 | position_cmd_sub_ = n.subscribe("position_cmd", 10, &PIDControlNodelet::position_cmd_callback, this, 166 | ros::TransportHints().tcpNoDelay()); 167 | 168 | enable_motors_sub_ = 169 | n.subscribe("motors", 2, &PIDControlNodelet::enable_motors_callback, this, ros::TransportHints().tcpNoDelay()); 170 | } 171 | 172 | #include 173 | PLUGINLIB_EXPORT_CLASS(PIDControlNodelet, nodelet::Nodelet); 174 | -------------------------------------------------------------------------------- /kr_mav_controllers/test/so3_control_nodelet.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /kr_mav_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mav_launch) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | find_package(catkin REQUIRED) 10 | catkin_package() 11 | 12 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 13 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 14 | # install(DIRECTORY scripts/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts) 15 | -------------------------------------------------------------------------------- /kr_mav_launch/config/gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 7.4, y: 7.4, z: 10.4} 3 | vel: {x: 4.8, y: 4.8, z: 6.0} 4 | ki: {x: 0.00, y: 0.00, z: 0.00} 5 | kib: {x: 0.00, y: 0.00, z: 0.00} 6 | rot: {x: 1.5, y: 1.5, z: 1.0} 7 | ang: {x: 0.13, y: 0.13, z: 0.1} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 16 | max_tilt_angle: 3.14 17 | -------------------------------------------------------------------------------- /kr_mav_launch/config/mav_manager_params.yaml: -------------------------------------------------------------------------------- 1 | server_wait_timeout: 0.5 2 | need_imu: false 3 | need_output_data: true 4 | use_attitude_safety_catch: false 5 | max_attitude_angle: 0.43 6 | takeoff_height: 0.2 -------------------------------------------------------------------------------- /kr_mav_launch/config/tracker_params.yaml: -------------------------------------------------------------------------------- 1 | # This should contain tracker parameters 2 | 3 | line_tracker_distance: 4 | default_v_des: 2.0 5 | default_a_des: 1.0 6 | epsilon: 0.1 7 | 8 | line_tracker_min_jerk: 9 | default_v_des: 2.0 10 | default_a_des: 1.0 11 | default_yaw_v_des: 0.8 12 | default_yaw_a_des: 0.2 13 | 14 | trajectory_tracker: 15 | max_vel_des: 2.0 16 | max_acc_des: 2.0 17 | 18 | velocity_tracker: 19 | timeout: 0.5 20 | 21 | lissajous_tracker: 22 | frame_id: simulator 23 | 24 | lissajous_adder: 25 | frame_id: simulator -------------------------------------------------------------------------------- /kr_mav_launch/config/trackers.yaml: -------------------------------------------------------------------------------- 1 | trackers: 2 | - kr_trackers/LineTrackerMinJerk 3 | - kr_trackers/LineTrackerDistance 4 | # - kr_trackers/LineTrackerTrapezoid 5 | # - kr_trackers/LineTrackerYaw 6 | - kr_trackers/VelocityTracker 7 | - kr_trackers/NullTracker 8 | - kr_trackers/CircleTracker 9 | - kr_trackers/TrajectoryTracker 10 | - kr_trackers/SmoothVelTracker 11 | - kr_trackers/LissajousTracker 12 | - kr_trackers/LissajousAdder 13 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/asctec_comms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 13 | 14 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 31 | 32 | 33 | 34 | 35 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/demo.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 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/example_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 20 | 21 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/mesh_vis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | -------------------------------------------------------------------------------- /kr_mav_launch/launch/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 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 | -------------------------------------------------------------------------------- /kr_mav_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_launch 3 | 1.0.0 4 | kr_mav_launch 5 | Dinesh Thakur 6 | 7 | BSD 8 | 9 | Dinesh Thakur 10 | 11 | 12 | catkin 13 | 14 | kr_mav_manager 15 | kr_mesh_visualization 16 | kr_quadrotor_simulator 17 | kr_mav_controllers 18 | kr_trackers 19 | kr_trackers_manager 20 | nodelet 21 | rviz 22 | 23 | 24 | -------------------------------------------------------------------------------- /kr_mav_launch/scripts/sample.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $# -eq 0 ]; then 4 | MAV_NAME=quadrotor 5 | echo "Using default mav_name $MAV_NAME" 6 | else 7 | MAV_NAME=$1 8 | fi 9 | 10 | echo "Enable motors..." 11 | rosservice call /$MAV_NAME/mav_services/motors true 12 | sleep 1 13 | 14 | read -p "Press [Enter] to takeoff" 15 | echo "Takeoff..." 16 | rosservice call /$MAV_NAME/mav_services/takeoff 17 | sleep 1 18 | 19 | read -p "Press [Enter] to go to [1, 1, 1]" 20 | rosservice call /$MAV_NAME/mav_services/goTo "goal: [1.0, 1.0, 1.0, 0.0]" 21 | sleep 1 22 | 23 | read -p "Press [Enter] to hover" 24 | rosservice call /$MAV_NAME/mav_services/hover 25 | sleep 1 26 | 27 | read -p "Press [Enter] to ascend at 0.5 m/s" 28 | rosservice call /$MAV_NAME/mav_services/setDesVelInWorldFrame "goal: [0.0, 0.0, 0.5, 0.0]" 29 | sleep 1 30 | 31 | read -p "Press [Enter] to hover" 32 | rosservice call /$MAV_NAME/mav_services/hover 33 | sleep 1 34 | 35 | read -p "Press [Enter] to go home" 36 | rosservice call /$MAV_NAME/mav_services/goHome 37 | sleep 1 38 | 39 | read -p "Press [Enter] to land" 40 | rosservice call /$MAV_NAME/mav_services/land 41 | sleep 1 42 | 43 | read -p "Press [Enter] to estop" 44 | rosservice call /$MAV_NAME/mav_services/estop 45 | -------------------------------------------------------------------------------- /kr_mav_launch/scripts/takeoff.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $# -eq 0 ]; then 4 | echo "Input mav name as argument" 5 | exit 1 6 | fi 7 | MAV_NAME=$1 8 | 9 | rostopic pub -1 /$MAV_NAME/motors std_msgs/Bool 0 10 | rostopic pub -1 /$MAV_NAME/trackers_manager/line_tracker_min_jerk/goal kr_mav_msgs/LineTrackerGoal "{x: 0.5, y: 0.5, z: 2.0, yaw: 0.0, v_des: 0.0, a_des: 0.0}" 11 | rosservice call /$MAV_NAME/trackers_manager/transition kr_trackers/LineTrackerMinJerk 12 | rostopic pub -1 /$MAV_NAME/motors std_msgs/Bool 1 13 | -------------------------------------------------------------------------------- /kr_mav_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mav_manager) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS roscpp 17 | nav_msgs 18 | sensor_msgs 19 | geometry_msgs 20 | kr_mav_msgs 21 | kr_tracker_msgs 22 | std_msgs 23 | std_srvs 24 | message_generation 25 | actionlib) 26 | find_package(Eigen3 REQUIRED NO_MODULE) 27 | 28 | add_service_files( 29 | DIRECTORY 30 | srv 31 | FILES 32 | Vec4.srv 33 | GoalTimed.srv 34 | Circle.srv 35 | Lissajous.srv 36 | CompoundLissajous.srv) 37 | generate_messages() 38 | 39 | catkin_package( 40 | INCLUDE_DIRS 41 | include 42 | LIBRARIES 43 | ${PROJECT_NAME} 44 | CATKIN_DEPENDS 45 | roscpp 46 | std_msgs 47 | nav_msgs 48 | sensor_msgs 49 | geometry_msgs 50 | kr_mav_msgs 51 | kr_tracker_msgs 52 | message_runtime 53 | DEPENDS 54 | EIGEN3) 55 | 56 | add_library(${PROJECT_NAME} src/manager.cpp) 57 | target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) 58 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 59 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 60 | 61 | add_executable(mav_services src/mav_services.cpp) 62 | target_link_libraries(mav_services PRIVATE ${PROJECT_NAME}) 63 | 64 | install( 65 | TARGETS ${PROJECT_NAME} mav_services 66 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 69 | 70 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 71 | -------------------------------------------------------------------------------- /kr_mav_manager/README.md: -------------------------------------------------------------------------------- 1 | kr_mav_manager 2 | ============= 3 | 4 | A manager for the kr_mav_control stack. 5 | This should make it easier to quickly generate state controllers using the 6 | tools in this package. 7 | 8 | -------------------------------------------------------------------------------- /kr_mav_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_manager 3 | 1.0.0 4 | 5 | This package provides tools to interface with the kr_mav_control stack 6 | 7 | Justin Thomas 8 | BSD 9 | 10 | catkin 11 | 12 | message_generation 13 | 14 | roscpp 15 | geometry_msgs 16 | nav_msgs 17 | std_msgs 18 | sensor_msgs 19 | kr_mav_msgs 20 | kr_tracker_msgs 21 | std_srvs 22 | actionlib 23 | 24 | message_runtime 25 | 26 | 27 | -------------------------------------------------------------------------------- /kr_mav_manager/src/mav_services.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "manager"); 6 | ros::NodeHandle nh; 7 | 8 | auto mav = std::make_shared(); 9 | 10 | kr_mav_manager::MAVManagerServices mm_srvs(mav); 11 | 12 | ros::spin(); 13 | 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /kr_mav_manager/src/sample_sub.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // Services 8 | ros::Subscriber sub_motors_, sub_takeoff_, sub_goTo_, sub_setDesVelInWorldFrame_, sub_setDesVelInBodyFrame_, sub_hover_, 9 | sub_ehover_, sub_estop_; 10 | 11 | // Typedefs 12 | typedef Eigen::Vector4d Vec4; 13 | 14 | class MAV_Subscribers 15 | { 16 | public: 17 | // Let's make an MAV 18 | MAVManager mav_; 19 | 20 | void motors_cb(const std_msgs::Bool &msg) { mav_.set_motors(msg.data); } 21 | void takeoff_cb(const std_msgs::Empty &msg) 22 | { 23 | ROS_INFO("Trying to takeoff"); 24 | if(!mav_.takeoff()) 25 | { 26 | ROS_ERROR("Takeoff failed"); 27 | } 28 | } 29 | void goTo_cb(const geometry_msgs::Pose &msg) 30 | { 31 | if(!mav_.goTo(msg.position.x, msg.position.y, msg.position.z, tf::getYaw(msg.orientation))) 32 | { 33 | ROS_ERROR("GoTo failed"); 34 | } 35 | } 36 | void setDesVelInWorldFrame_cb(const geometry_msgs::Twist &msg) 37 | { 38 | mav_.setDesVelInWorldFrame(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z); 39 | } 40 | void setDesVelInBodyFrame_cb(const geometry_msgs::Twist &msg) 41 | { 42 | mav_.setDesVelInBodyFrame(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z); 43 | } 44 | void hover_cb(const std_msgs::Empty &msg) 45 | { 46 | if(!mav_.hover()) 47 | { 48 | ROS_ERROR("Hover failed"); 49 | } 50 | } 51 | void ehover_cb(const std_msgs::Empty &msg) 52 | { 53 | if(!mav_.ehover()) 54 | { 55 | ROS_ERROR("Ehover failed"); 56 | } 57 | } 58 | void estop_cb(const std_msgs::Empty &msg) { mav_.estop(); } 59 | }; 60 | 61 | int main(int argc, char **argv) 62 | { 63 | ros::init(argc, argv, "manager"); 64 | ros::NodeHandle nh("~"); 65 | 66 | MAV_Subscribers mav_subs; 67 | 68 | // Services 69 | sub_motors_ = nh.subscribe("motors", 10, &MAV_Subscribers::motors_cb, &mav_subs); 70 | sub_takeoff_ = nh.subscribe("takeoff", 10, &MAV_Subscribers::takeoff_cb, &mav_subs); 71 | sub_goTo_ = nh.subscribe("goTo", 10, &MAV_Subscribers::goTo_cb, &mav_subs); 72 | sub_setDesVelInWorldFrame_ = 73 | nh.subscribe("setDesVelInWorldFrame", 10, &MAV_Subscribers::setDesVelInWorldFrame_cb, &mav_subs); 74 | sub_setDesVelInBodyFrame_ = 75 | nh.subscribe("setDesVelInBodyFrame", 10, &MAV_Subscribers::setDesVelInBodyFrame_cb, &mav_subs); 76 | sub_hover_ = nh.subscribe("hover", 10, &MAV_Subscribers::hover_cb, &mav_subs); 77 | sub_ehover_ = nh.subscribe("ehover", 10, &MAV_Subscribers::ehover_cb, &mav_subs); 78 | sub_estop_ = nh.subscribe("estop", 10, &MAV_Subscribers::estop_cb, &mav_subs); 79 | 80 | // Let's spin some rotors 81 | ros::spin(); 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /kr_mav_manager/srv/Circle.srv: -------------------------------------------------------------------------------- 1 | float32 Ax 2 | float32 Ay 3 | float32 T 4 | float32 duration 5 | --- 6 | bool success 7 | string message 8 | -------------------------------------------------------------------------------- /kr_mav_manager/srv/CompoundLissajous.srv: -------------------------------------------------------------------------------- 1 | float32[2] x_amp 2 | float32[2] y_amp 3 | float32[2] z_amp 4 | float32[2] yaw_amp 5 | float32[2] x_num_periods 6 | float32[2] y_num_periods 7 | float32[2] z_num_periods 8 | float32[2] yaw_num_periods 9 | float32[2] period 10 | float32[2] num_cycles 11 | float32[2] ramp_time 12 | --- 13 | bool success 14 | string message 15 | -------------------------------------------------------------------------------- /kr_mav_manager/srv/GoalTimed.srv: -------------------------------------------------------------------------------- 1 | float32[4] goal 2 | duration duration 3 | time t_start 4 | --- 5 | bool success 6 | string message 7 | -------------------------------------------------------------------------------- /kr_mav_manager/srv/Lissajous.srv: -------------------------------------------------------------------------------- 1 | float32 x_amp 2 | float32 y_amp 3 | float32 z_amp 4 | float32 yaw_amp 5 | float32 x_num_periods 6 | float32 y_num_periods 7 | float32 z_num_periods 8 | float32 yaw_num_periods 9 | float32 period 10 | float32 num_cycles 11 | float32 ramp_time 12 | --- 13 | bool success 14 | string message 15 | -------------------------------------------------------------------------------- /kr_mav_manager/srv/Vec4.srv: -------------------------------------------------------------------------------- 1 | float32[4] goal 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /kr_mav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mav_msgs) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS message_generation geometry_msgs) 10 | find_package(Eigen3 REQUIRED) 11 | 12 | add_message_files( 13 | DIRECTORY 14 | msg 15 | FILES 16 | AuxCommand.msg 17 | Corrections.msg 18 | MotorRPM.msg 19 | OutputData.msg 20 | PWMCommand.msg 21 | PositionCommand.msg 22 | SO3Command.msg 23 | Serial.msg 24 | StatusData.msg 25 | TRPYCommand.msg) 26 | 27 | generate_messages(DEPENDENCIES geometry_msgs) 28 | 29 | catkin_package( 30 | CATKIN_DEPENDS 31 | geometry_msgs 32 | message_runtime 33 | DEPENDS 34 | EIGEN3) 35 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/AuxCommand.msg: -------------------------------------------------------------------------------- 1 | float64 current_yaw 2 | float64 kf_correction 3 | float64[2] angle_corrections # Trims for roll, pitch 4 | bool enable_motors 5 | bool use_external_yaw 6 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/Corrections.msg: -------------------------------------------------------------------------------- 1 | float64 kf_correction 2 | float64[2] angle_corrections 3 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/MotorRPM.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 motor_count 3 | int16[8] rpm 4 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/OutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | geometry_msgs/Quaternion orientation 5 | geometry_msgs/Vector3 angular_velocity 6 | geometry_msgs/Vector3 linear_acceleration 7 | float64 pressure_dheight 8 | float64 pressure_height 9 | geometry_msgs/Vector3 magnetic_field 10 | uint8[8] radio_channel 11 | int16[4] motor_rpm 12 | uint8 seq 13 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/PWMCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | # pwm values should be between 0 (1ms pulse width) and 1 (2ms pulse width) 3 | float64[2] pwm 4 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/PositionCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point position 3 | geometry_msgs/Vector3 velocity 4 | geometry_msgs/Vector3 acceleration 5 | geometry_msgs/Vector3 jerk 6 | float64 yaw 7 | float64 yaw_dot 8 | float64[3] kx 9 | float64[3] kv 10 | 11 | # use_msg_gains_flags: flags that can be set using the constants USE_MSG_GAINS_* 12 | # to select which gains (kx, kv) from the message to use. Note that by default 13 | # the message gains are not used. 14 | uint8 use_msg_gains_flags 15 | 16 | uint8 USE_MSG_GAINS_NONE = 0 17 | uint8 USE_MSG_GAINS_POSITION_X = 1 18 | uint8 USE_MSG_GAINS_POSITION_Y = 2 19 | uint8 USE_MSG_GAINS_POSITION_Z = 4 20 | uint8 USE_MSG_GAINS_POSITION_ALL = 7 # sets all three of the above 21 | uint8 USE_MSG_GAINS_VELOCITY_X = 8 22 | uint8 USE_MSG_GAINS_VELOCITY_Y = 16 23 | uint8 USE_MSG_GAINS_VELOCITY_Z = 32 24 | uint8 USE_MSG_GAINS_VELOCITY_ALL = 56 # sets all three of the above 25 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/SO3Command.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 force 3 | geometry_msgs/Quaternion orientation 4 | geometry_msgs/Vector3 angular_velocity 5 | kr_mav_msgs/AuxCommand aux 6 | float64[3] kR 7 | float64[3] kOm 8 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/Serial.msg: -------------------------------------------------------------------------------- 1 | # Note: These constants need to be kept in sync with the types 2 | # defined in include/kr_mav_msgs/comm_types.h 3 | uint8 SO3_CMD = 115 # 's' in base 10 4 | uint8 TRPY_CMD = 112 # 'p' in base 10 5 | uint8 PWM_CMD = 119 # 'w' in base 10 6 | uint8 STATUS_DATA = 99 # 'c' in base 10 7 | uint8 OUTPUT_DATA = 100 # 'd' in base 10 8 | 9 | Header header 10 | uint8 channel 11 | uint8 type # One of the types listed above 12 | uint8[] data 13 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/StatusData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | uint8 seq 5 | -------------------------------------------------------------------------------- /kr_mav_msgs/msg/TRPYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 thrust 3 | float64 roll 4 | float64 pitch 5 | float64 yaw 6 | geometry_msgs/Vector3 angular_velocity 7 | float64[3] kR 8 | float64[3] kOm 9 | kr_mav_msgs/AuxCommand aux 10 | -------------------------------------------------------------------------------- /kr_mav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mav_msgs 3 | 1.0.0 4 | kr_mav_msgs 5 | Kartik Mohta 6 | Michael Watterson 7 | Justin Thomas 8 | 9 | BSD 10 | 11 | Kartik Mohta 12 | 13 | catkin 14 | 15 | geometry_msgs 16 | message_generation 17 | message_runtime 18 | 19 | 20 | -------------------------------------------------------------------------------- /kr_mesh_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_mesh_visualization) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs topic_tools visualization_msgs) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS 18 | geometry_msgs 19 | nav_msgs 20 | topic_tools 21 | visualization_msgs) 22 | 23 | add_executable(${PROJECT_NAME} src/mesh_visualization.cpp) 24 | target_include_directories(${PROJECT_NAME} PRIVATE ${catkin_INCLUDE_DIRS}) 25 | target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES}) 26 | 27 | install( 28 | TARGETS ${PROJECT_NAME} 29 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 30 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 32 | 33 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 34 | install(DIRECTORY mesh/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mesh) 35 | -------------------------------------------------------------------------------- /kr_mesh_visualization/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kr_mesh_visualization/mesh/hummingbird.mesh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/kr_mesh_visualization/mesh/hummingbird.mesh -------------------------------------------------------------------------------- /kr_mesh_visualization/mesh/nano.mesh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_mav_control/e9b25e3c95efc9888905b461ec5a44be7447b225/kr_mesh_visualization/mesh/nano.mesh -------------------------------------------------------------------------------- /kr_mesh_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_mesh_visualization 3 | 1.0.0 4 | kr_mesh_visualization 5 | 6 | Kartik Mohta 7 | Michael Watterson 8 | 9 | BSD 10 | 11 | Kartik Mohta 12 | 13 | catkin 14 | 15 | nav_msgs 16 | geometry_msgs 17 | topic_tools 18 | visualization_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /kr_mesh_visualization/src/mesh_visualization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | static std::string mesh_resource, new_frame_id; 9 | static ros::Publisher pub_vis; 10 | static double color_r, color_g, color_b, color_a; 11 | static double scale_x, scale_y, scale_z; 12 | 13 | static void publishMarker(const std::string &frame_id, const geometry_msgs::Pose &pose) 14 | { 15 | visualization_msgs::Marker marker; 16 | marker.header.frame_id = (new_frame_id == "") ? frame_id : new_frame_id; 17 | marker.header.stamp = ros::Time(); // time 0 so that the marker will be 18 | // displayed regardless of the current time 19 | marker.ns = ros::this_node::getName(); 20 | marker.id = 0; 21 | marker.type = visualization_msgs::Marker::MESH_RESOURCE; 22 | marker.action = visualization_msgs::Marker::ADD; 23 | marker.pose = pose; 24 | marker.scale.x = scale_x; 25 | marker.scale.y = scale_y; 26 | marker.scale.z = scale_z; 27 | marker.color.a = color_a; 28 | marker.color.r = color_r; 29 | marker.color.g = color_g; 30 | marker.color.b = color_b; 31 | marker.mesh_resource = mesh_resource; 32 | pub_vis.publish(marker); 33 | } 34 | 35 | static void any_callback(const topic_tools::ShapeShifter::ConstPtr &msg) 36 | { 37 | // ROS_INFO_STREAM("Type: " << msg->getDataType()); 38 | 39 | if(msg->getDataType() == "geometry_msgs/PoseStamped") 40 | { 41 | auto pose_msg = msg->instantiate(); 42 | publishMarker(pose_msg->header.frame_id, pose_msg->pose); 43 | } 44 | else if(msg->getDataType() == "geometry_msgs/PoseWithCovarianceStamped") 45 | { 46 | auto pose_msg = msg->instantiate(); 47 | publishMarker(pose_msg->header.frame_id, pose_msg->pose.pose); 48 | } 49 | else if(msg->getDataType() == "nav_msgs/Odometry") 50 | { 51 | auto odom_msg = msg->instantiate(); 52 | publishMarker(odom_msg->header.frame_id, odom_msg->pose.pose); 53 | } 54 | else 55 | { 56 | ROS_ERROR_STREAM(ros::this_node::getName() << " got unsupported message type " << msg->getDataType()); 57 | } 58 | } 59 | 60 | int main(int argc, char **argv) 61 | { 62 | ros::init(argc, argv, "kr_mesh_visualization"); 63 | 64 | ros::NodeHandle nh("~"); 65 | 66 | nh.param("mesh_resource", mesh_resource, std::string("package://kr_mesh_visualization/mesh/hummingbird.mesh")); 67 | nh.param("color/r", color_r, 1.0); 68 | nh.param("color/g", color_g, 0.0); 69 | nh.param("color/b", color_b, 0.0); 70 | nh.param("color/a", color_a, 1.0); 71 | nh.param("scale/x", scale_x, 1.0); 72 | nh.param("scale/y", scale_y, 1.0); 73 | nh.param("scale/z", scale_z, 1.0); 74 | 75 | nh.param("new_frame_id", new_frame_id, std::string("")); 76 | 77 | pub_vis = nh.advertise("robot", 10); 78 | 79 | ros::Subscriber any_sub = nh.subscribe("input", 10, &any_callback, ros::TransportHints().tcpNoDelay()); 80 | ros::spin(); 81 | 82 | return 0; 83 | } 84 | -------------------------------------------------------------------------------- /kr_quadrotor_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_quadrotor_simulator) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package(catkin REQUIRED COMPONENTS geometry_msgs kr_mav_msgs nav_msgs roscpp sensor_msgs tf2_ros) 15 | find_package(Eigen3 REQUIRED) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS 19 | geometry_msgs 20 | kr_mav_msgs 21 | nav_msgs 22 | roscpp 23 | sensor_msgs 24 | tf2_ros 25 | DEPENDS 26 | EIGEN3 27 | ) 28 | 29 | add_library(${PROJECT_NAME} INTERFACE) 30 | target_include_directories(${PROJECT_NAME} INTERFACE ${catkin_INCLUDE_DIRS}) 31 | target_link_libraries(${PROJECT_NAME} INTERFACE ${catkin_LIBRARIES}) 32 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 33 | 34 | add_library(kr_quadrotor_dynamics src/dynamics/Quadrotor.cpp) 35 | target_include_directories(kr_quadrotor_dynamics PUBLIC include) 36 | target_link_libraries(kr_quadrotor_dynamics PUBLIC Eigen3::Eigen ${PROJECT_NAME}) 37 | 38 | add_executable(${PROJECT_NAME}_so3 src/quadrotor_simulator_so3.cpp) 39 | target_link_libraries(${PROJECT_NAME}_so3 PUBLIC kr_quadrotor_dynamics) 40 | # add_dependencies(${PROJECT_NAME}_so3 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 41 | 42 | add_executable(${PROJECT_NAME}_trpy src/quadrotor_simulator_trpy.cpp) 43 | target_link_libraries(${PROJECT_NAME}_trpy PUBLIC kr_quadrotor_dynamics) 44 | # add_dependencies(${PROJECT_NAME}_trpy ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 45 | 46 | install( 47 | TARGETS kr_quadrotor_dynamics ${PROJECT_NAME}_so3 ${PROJECT_NAME}_trpy 48 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 51 | 52 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 53 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 54 | -------------------------------------------------------------------------------- /kr_quadrotor_simulator/config/crazyflie_params.yaml: -------------------------------------------------------------------------------- 1 | gravity: 9.81 2 | mass: 0.029 3 | prop_radius: 0.0225 4 | arm_length: 0.046 5 | motor_time_constant: 0.00001 # TODO 6 | min_rpm: 0 # TODO 7 | max_rpm: 10000 #TODO 8 | # The following values are from: 9 | # http://groups.csail.mit.edu/robotics-center/public_papers/Landry15.pdf 10 | Ixx: 2.395e-5 11 | Iyy: 2.395e-5 12 | Izz: 3.235e-5 13 | thrust_coefficient: 5.507e-5 # (0.005022 N s^2 / rad^2) * (1 min / 60 s)^2 * (2*pi rad / rev)^2 14 | -------------------------------------------------------------------------------- /kr_quadrotor_simulator/config/hummingbird_params.yaml: -------------------------------------------------------------------------------- 1 | gravity: 9.81 2 | mass: 0.5 3 | Ixx: 2.64e-3 4 | Iyy: 2.64e-3 5 | Izz: 4.96e-3 6 | thrust_coefficient: 5.55e-8 7 | prop_radius: 0.099 8 | arm_length: 0.17 9 | motor_time_constant: 0.05 10 | min_rpm: 1500 11 | max_rpm: 7500 12 | drag_coefficient: 0.0 13 | -------------------------------------------------------------------------------- /kr_quadrotor_simulator/config/qualcomm_quad_params.yaml: -------------------------------------------------------------------------------- 1 | gravity: 9.81 2 | mass: 0.25 3 | Ixx: 0.000601 4 | Iyy: 0.000589 5 | Izz: 0.001076 6 | thrust_coefficient: 4.1790e-09 7 | prop_radius: 0.05 8 | arm_length: 0.1075 9 | motor_time_constant: 0.05 10 | min_rpm: 5500 11 | max_rpm: 16400 12 | drag_coefficient: 0.0 -------------------------------------------------------------------------------- /kr_quadrotor_simulator/include/kr_quadrotor_simulator/Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADROTOR_SIMULATOR_QUADROTOR_H 2 | #define QUADROTOR_SIMULATOR_QUADROTOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace QuadrotorSimulator 8 | { 9 | class Quadrotor 10 | { 11 | public: 12 | struct State 13 | { 14 | Eigen::Vector3d x; 15 | Eigen::Vector3d v; 16 | Eigen::Matrix3d R; 17 | Eigen::Vector3d omega; 18 | Eigen::Array4d motor_rpm; 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 20 | }; 21 | 22 | Quadrotor(); 23 | 24 | const Quadrotor::State &getState() const; 25 | void setState(const Quadrotor::State &state); 26 | 27 | double getMass() const; 28 | void setMass(double mass); 29 | 30 | double getDragCoefficient() const; 31 | void setDragCoefficient(double drag_coefficient); 32 | 33 | double getGravity() const; 34 | void setGravity(double g); 35 | 36 | const Eigen::Matrix3d &getInertia() const; 37 | void setInertia(const Eigen::Matrix3d &inertia); 38 | 39 | double getArmLength() const; 40 | void setArmLength(double d); 41 | 42 | double getPropRadius() const; 43 | void setPropRadius(double r); 44 | 45 | double getPropellerThrustCoefficient() const; 46 | void setPropellerThrustCoefficient(double kf); 47 | 48 | double getPropellerMomentCoefficient() const; 49 | void setPropellerMomentCoefficient(double km); 50 | 51 | double getMotorTimeConstant() const; 52 | void setMotorTimeConstant(double k); 53 | 54 | const Eigen::Vector3d &getExternalForce() const; 55 | void setExternalForce(const Eigen::Vector3d &force); 56 | 57 | const Eigen::Vector3d &getExternalMoment() const; 58 | void setExternalMoment(const Eigen::Vector3d &moment); 59 | 60 | double getMaxRPM() const; 61 | void setMaxRPM(double max_rpm); 62 | 63 | double getMinRPM() const; 64 | void setMinRPM(double min_rpm); 65 | 66 | // Inputs are desired RPM for the motors 67 | // Rotor numbering is: 68 | // *1* Front 69 | // 3 4 70 | // 2 71 | // with 1 and 2 clockwise and 3 and 4 counter-clockwise (looking from top) 72 | void setInput(double u1, double u2, double u3, double u4); 73 | 74 | // Runs the actual dynamics simulation with a time step of dt 75 | void step(double dt); 76 | 77 | // For internal use, but needs to be public for odeint 78 | typedef boost::array InternalState; 79 | void operator()(const Quadrotor::InternalState &x, Quadrotor::InternalState &dxdt, const double /* t */); 80 | 81 | private: 82 | void updateInternalState(); 83 | 84 | double g_; // gravity 85 | double mass_; 86 | Eigen::Matrix3d J_; // Inertia 87 | double kf_; 88 | double km_; 89 | double prop_radius_; 90 | double arm_length_; 91 | double motor_time_constant_; // unit: sec 92 | double max_rpm_; 93 | double min_rpm_; 94 | double drag_coefficient_; 95 | Quadrotor::State state_; 96 | Eigen::Array4d input_; 97 | Eigen::Vector3d external_force_; 98 | Eigen::Vector3d external_moment_; 99 | 100 | InternalState internal_state_; 101 | }; 102 | 103 | } // namespace QuadrotorSimulator 104 | #endif 105 | -------------------------------------------------------------------------------- /kr_quadrotor_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_quadrotor_simulator 3 | 1.0.0 4 | kr_quadrotor_simulator 5 | Kartik Mohta 6 | Michael Watterson 7 | 8 | BSD 9 | 10 | Kartik Mohta 11 | 12 | catkin 13 | 14 | geometry_msgs 15 | kr_mav_msgs 16 | nav_msgs 17 | roscpp 18 | sensor_msgs 19 | tf2_ros 20 | 21 | -------------------------------------------------------------------------------- /rqt_mav_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rqt_mav_manager) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_python_setup() 6 | 7 | catkin_package() 8 | 9 | install(FILES plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | 11 | install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | install(PROGRAMS scripts/rqt_mav_manager DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 14 | -------------------------------------------------------------------------------- /rqt_mav_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rqt_mav_manager 3 | 0.1.0 4 | Provides a GUI plugin for MAV Manager. 5 | Dinesh Thakur 6 | Kartik Mohta 7 | 8 | BSD 9 | 10 | https://github.com/KumarRobotics/kr_ui 11 | 12 | Dinesh Thakur 13 | 14 | catkin 15 | 16 | rospy 17 | rqt_gui_py 18 | kr_mav_manager 19 | std_srvs 20 | rqt_gui 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rqt_mav_manager/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A Python GUI plugin to control a quadrotor using the kr_mav_manager interface. 5 | 6 | 7 | 8 | 9 | folder 10 | Plugins related to robot tools. 11 | 12 | 13 | input-gaming 14 | A Python GUI plugin for controlling a quadrotor using the kr_mav_manager. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rqt_mav_manager/scripts/rqt_mav_manager: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_mav_manager.MavManagerUi')) 9 | -------------------------------------------------------------------------------- /rqt_mav_manager/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['rqt_mav_manager'], 6 | package_dir={'': 'src'}, 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_tracker_msgs) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | find_package(catkin REQUIRED COMPONENTS message_generation nav_msgs geometry_msgs actionlib_msgs) 10 | 11 | add_service_files(DIRECTORY srv FILES Transition.srv) 12 | 13 | add_action_files( 14 | DIRECTORY 15 | action 16 | FILES 17 | LineTracker.action 18 | VelocityTracker.action 19 | CircleTracker.action 20 | TrajectoryTracker.action 21 | LissajousTracker.action 22 | LissajousAdder.action) 23 | 24 | add_message_files( 25 | DIRECTORY 26 | msg 27 | FILES 28 | TrackerStatus.msg 29 | VelocityGoal.msg) 30 | 31 | generate_messages(DEPENDENCIES geometry_msgs actionlib_msgs) 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS 35 | nav_msgs 36 | geometry_msgs 37 | actionlib_msgs 38 | message_runtime) 39 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/CircleTracker.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float64 Ax 3 | float64 Ay 4 | float64 T 5 | float64 duration 6 | --- 7 | #result definition 8 | # time duration of the trajectory 9 | float64 duration 10 | # approximate arc length of the trajectory (ie. sum of distances between position commands) 11 | float64 length 12 | --- 13 | #feedback 14 | # time in tracker 15 | float64 duration -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/LineTracker.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float64 x 3 | float64 y 4 | float64 z 5 | float64 yaw 6 | float64 v_des 7 | float64 a_des 8 | bool relative 9 | time t_start 10 | duration duration 11 | --- 12 | #result definition 13 | # send back goal 14 | float64 x 15 | float64 y 16 | float64 z 17 | float64 yaw 18 | # time duration of the trajectory 19 | float64 duration 20 | # approximate arc length of the trajectory (ie. sum of distances between position commands) 21 | float64 length 22 | --- 23 | #feedback 24 | # distance from the goal 25 | float64 distance_from_goal 26 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/LissajousAdder.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float64[2] x_amp 3 | float64[2] y_amp 4 | float64[2] z_amp 5 | float64[2] yaw_amp 6 | float64[2] x_num_periods 7 | float64[2] y_num_periods 8 | float64[2] z_num_periods 9 | float64[2] yaw_num_periods 10 | float64[2] period 11 | float64[2] num_cycles 12 | float64[2] ramp_time 13 | --- 14 | #result definition 15 | # send back goal 16 | float64 x 17 | float64 y 18 | float64 z 19 | float64 yaw 20 | # time duration of the trajectory 21 | float64 duration 22 | # approximate arc length of the trajectory (ie. sum of distances between position commands) 23 | float64 length 24 | --- 25 | #feedback 26 | # distance from the goal 27 | float64 time_to_completion 28 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/LissajousTracker.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float64 x_amp 3 | float64 y_amp 4 | float64 z_amp 5 | float64 yaw_amp 6 | float64 x_num_periods 7 | float64 y_num_periods 8 | float64 z_num_periods 9 | float64 yaw_num_periods 10 | float64 period 11 | float64 num_cycles 12 | float64 ramp_time 13 | --- 14 | #result definition 15 | # send back goal 16 | float64 x 17 | float64 y 18 | float64 z 19 | float64 yaw 20 | # time duration of the trajectory 21 | float64 duration 22 | # approximate arc length of the trajectory (ie. sum of distances between position commands) 23 | float64 length 24 | --- 25 | #feedback 26 | # distance from the goal 27 | float64 time_to_completion 28 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/TrajectoryTracker.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/Pose[] waypoints 3 | float64[] waypoint_times # If empty, waypoint times are computed 4 | --- 5 | #result definition 6 | float64 total_time 7 | float64 total_distance_travelled 8 | --- 9 | #feedback 10 | float64 remaining_time 11 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/action/VelocityTracker.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float64 vx 3 | float64 vy 4 | float64 vz 5 | float64 vyaw 6 | bool use_position_gains 7 | --- 8 | #result definition 9 | # send back goal 10 | float64 vx 11 | float64 vy 12 | float64 vz 13 | float64 vyaw 14 | bool use_position_gains 15 | # time duration of the trajectory 16 | float64 duration 17 | # approximate arc length of the trajectory (ie. sum of distances between position commands) 18 | float64 length 19 | --- 20 | #feedback 21 | # time in tracker 22 | float64 duration 23 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/msg/TrackerStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string tracker 3 | uint8 status 4 | 5 | # Options for the status 6 | uint8 ACTIVE = 0 # Currently active 7 | uint8 SUCCEEDED = 1 # The tracker has finished 8 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/msg/VelocityGoal.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 vx 3 | float64 vy 4 | float64 vz 5 | float64 vyaw 6 | bool use_position_gains -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kr_tracker_msgs 3 | 1.0.0 4 | kr_tracker_msgs 5 | Dinesh Thakur 6 | Kartik Mohta 7 | 8 | BSD 9 | 10 | Dinesh Thakur 11 | 12 | catkin 13 | 14 | nav_msgs 15 | geometry_msgs 16 | actionlib_msgs 17 | 18 | message_generation 19 | message_runtime 20 | 21 | -------------------------------------------------------------------------------- /trackers/kr_tracker_msgs/srv/Transition.srv: -------------------------------------------------------------------------------- 1 | string tracker 2 | --- 3 | bool success # indicate successful transition 4 | string message # informational, e.g. for error messages 5 | -------------------------------------------------------------------------------- /trackers/kr_trackers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_trackers) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS actionlib 17 | std_msgs 18 | geometry_msgs 19 | nav_msgs 20 | tf 21 | kr_mav_msgs 22 | kr_tracker_msgs 23 | kr_trackers_manager) 24 | find_package(Eigen3 REQUIRED) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS 28 | include 29 | LIBRARIES 30 | ${PROJECT_NAME} 31 | CATKIN_DEPENDS 32 | actionlib 33 | std_msgs 34 | geometry_msgs 35 | nav_msgs 36 | tf 37 | kr_mav_msgs 38 | kr_tracker_msgs 39 | kr_trackers_manager 40 | DEPENDS 41 | EIGEN3) 42 | 43 | add_library( 44 | ${PROJECT_NAME} 45 | src/initial_conditions.cpp 46 | src/circle_tracker_server.cpp 47 | src/initial_conditions.cpp 48 | src/line_tracker_distance_server.cpp 49 | src/line_tracker_min_jerk_server.cpp 50 | # src/line_tracker_trapezoid.cpp 51 | # src/line_tracker_yaw.cpp 52 | src/lissajous_adder_server.cpp 53 | src/lissajous_generator.cpp 54 | src/lissajous_tracker_server.cpp 55 | src/null_tracker.cpp 56 | src/smooth_vel_tracker_server.cpp 57 | src/trajectory_tracker.cpp 58 | src/traj_gen.cpp 59 | src/velocity_tracker.cpp) 60 | target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) 61 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 62 | 63 | install( 64 | TARGETS ${PROJECT_NAME} 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 68 | 69 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 70 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 71 | 72 | install(PROGRAMS 73 | scripts/twist_to_velocity_goal.py 74 | scripts/waypoints_to_action.py 75 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 76 | ) 77 | -------------------------------------------------------------------------------- /trackers/kr_trackers/include/kr_trackers/initial_conditions.h: -------------------------------------------------------------------------------- 1 | #ifndef STD_TRACKERS_INITIAL_CONDITIONS_H 2 | #define STD_TRACKERS_INITIAL_CONDITIONS_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | class InitialConditions 10 | { 11 | public: 12 | InitialConditions(); 13 | 14 | void set_from_cmd(const kr_mav_msgs::PositionCommand::ConstPtr &msg); 15 | void set_from_odom(const nav_msgs::Odometry::ConstPtr &msg); 16 | Eigen::Vector3f pos() const { return pos_; } 17 | Eigen::Vector3f vel() const { return vel_; } 18 | Eigen::Vector3f acc() const { return acc_; } 19 | Eigen::Vector3f jrk() const { return jrk_; } 20 | float yaw() const { return yaw_; } 21 | float yaw_dot() const { return yaw_dot_; } 22 | void reset(); 23 | 24 | private: 25 | Eigen::Vector3f pos_, vel_, acc_, jrk_; 26 | float yaw_, yaw_dot_; 27 | bool cmd_valid_; 28 | }; 29 | 30 | #endif // STD_TRACKERS_INITIAL_CONDITIONS_H 31 | -------------------------------------------------------------------------------- /trackers/kr_trackers/include/kr_trackers/lissajous_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef _LISSAJOUS_GENERATOR_H_ 2 | #define _LISSAJOUS_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class LissajousGenerator 12 | { 13 | public: 14 | LissajousGenerator(void); 15 | void setParams(const kr_tracker_msgs::LissajousTrackerGoal::ConstPtr &msg); 16 | void setParams(const kr_tracker_msgs::LissajousAdderGoal::ConstPtr &msg, int num); 17 | void generatePath(nav_msgs::Path &path, geometry_msgs::Point &initial_pt, double dt); 18 | const kr_mav_msgs::PositionCommand::Ptr getPositionCmd(void); 19 | bool activate(void); 20 | void deactivate(void); 21 | bool isActive(void); 22 | bool goalIsSet(void); 23 | bool status(void) const; 24 | float timeRemaining(void); 25 | float timeElapsed(void); 26 | 27 | private: 28 | double lissajous_period_, ramp_time_, total_time_, ramp_s_, total_s_, const_time_, period_; 29 | double x_amp_, y_amp_, z_amp_, yaw_amp_; 30 | double x_num_periods_, y_num_periods_, z_num_periods_, yaw_num_periods_; 31 | double a7_, a6_, a5_, a4_; 32 | double num_cycles_; 33 | bool active_, goal_set_, goal_reached_; 34 | ros::Time start_time_; 35 | }; 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /trackers/kr_trackers/include/kr_trackers/traj_gen.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TrajectoryGenerator 8 | { 9 | public: 10 | using Vec3f = Eigen::Vector3f; 11 | using vec_Vec3f = std::vector>; 12 | 13 | /** 14 | * @brief 15 | * 16 | * @param continuous_derivative_order The highest derivative that is continous 17 | * @param minimize_derivative The derivative to minimize 18 | */ 19 | TrajectoryGenerator(unsigned int continuous_derivative_order, unsigned int minimize_derivative); 20 | 21 | void setInitialConditions(const Vec3f &position, const vec_Vec3f &derivatives); 22 | void addWaypoint(const Vec3f &position); // Waypoint is X, Y, Z 23 | void clearWaypoints(void); 24 | std::vector computeTimesTrapezoidSpeed(float vel_des, float acc_des) const; 25 | std::vector computeTimesConstantSpeed(float avg_speed) const; 26 | bool calculate(const std::vector &waypoint_times_); 27 | bool getCommand(const float time, Vec3f &pos, Vec3f &vel, Vec3f &acc, Vec3f &jrk) const; 28 | 29 | void calcMaxPerSegment(std::vector &max_vel, std::vector &max_acc, std::vector &max_jrk) const; 30 | 31 | void optimizeWaypointTimes(const float max_vel, const float max_acc, const float max_jrk); 32 | 33 | const std::vector &getWaypointTimes() const; 34 | float getTotalTime() const; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | private: 39 | const unsigned int N_; 40 | const unsigned int R_; 41 | vec_Vec3f waypoints_, initial_derivatives_; 42 | std::vector> coefficients_; 43 | std::vector waypoint_times_; 44 | }; 45 | -------------------------------------------------------------------------------- /trackers/kr_trackers/matlab/line_tracker_min_jerk_numerical_issues.m: -------------------------------------------------------------------------------- 1 | function line_tracker_min_jerk_numerical_issues() 2 | % line_track_min_jerk_numerical_issues 3 | % 4 | % Written by Justin Thomas, 5/6/2016 5 | 6 | total_t = 10; 7 | 8 | % Boundary conditions 9 | bcs = [ 10 | 0; % xo 11 | 100; % xf 12 | 10; % vo 13 | 0; % vf 14 | 0; % ao 15 | 0]; % af 16 | 17 | x_old = current_method(total_t, bcs); 18 | x_new = new_method(total_t, bcs); 19 | 20 | %% Evaluate 21 | 22 | tvec = 0:total_t/100:total_t; 23 | 24 | eval_old = evaluate_poly(x_old, tvec); 25 | 26 | eval_new = evaluate_poly(x_new, tvec / total_t); 27 | 28 | % Scale position (not needed) 29 | eval_new(1,:) = eval_new(1,:); 30 | % Scale velocity 31 | eval_new(2,:) = 1 / total_t * eval_new(2,:); 32 | % Scale acceleration 33 | eval_new(3,:) = 1 / total_t^2 * eval_new(3,:); 34 | 35 | close 36 | figure; 37 | hold all; 38 | for idx = 1:3 39 | plot(tvec, eval_old(idx,:), '.'); 40 | plot(tvec, eval_new(idx,:), 'o'); 41 | end 42 | 43 | end 44 | 45 | function [x] = current_method(total_t, bcs) 46 | 47 | dt = total_t; 48 | dt2 = dt^2; 49 | dt3 = dt^3; 50 | dt4 = dt^4; 51 | dt5 = dt^5; 52 | 53 | % syms dt dt2 dt3 dt4 dt5 real; 54 | 55 | A = [ 1, 0, 0, 0, 0, 0; 56 | 1, dt, dt2, dt3, dt4, dt5; 57 | 0, 1, 0, 0, 0, 0; 58 | 0, 1, 2 * dt, 3 * dt2, 4 * dt3, 5 * dt4; 59 | 0, 0, 2, 0, 0, 0; 60 | 0, 0, 2, 6 * dt, 12 * dt2, 20 * dt3]; 61 | 62 | disp(['Old method, condition number:', num2str(cond(A))]); 63 | 64 | x = A \ bcs; 65 | 66 | end 67 | 68 | function x = new_method(total_t, bcs) 69 | 70 | % Assume dt = 1 71 | A = [ 1, 0, 0, 0, 0, 0; 72 | 1, 1, 1, 1, 1, 1; 73 | 0, 1, 0, 0, 0, 0; 74 | 0, 1, 2, 3, 4, 5; 75 | 0, 0, 2, 0, 0, 0; 76 | 0, 0, 2, 6, 12, 20]; 77 | 78 | % To get a non-numerical solution to inv(A) 79 | % syms a b c d e f g h real; 80 | % A = [ a, 0, 0, 0, 0, 0; 81 | % a, a, a, a, a, a; 82 | % 0, a, 0, 0, 0, 0; 83 | % 0, a, b, c, d, e; 84 | % 0, 0, b, 0, 0, 0; 85 | % 0, 0, b, f, g, h]; 86 | % Ainv = subs(simplify(inv(A)), {'a','b','c','d','e','f','g','h'}, {1,2,3,4,5,6,12,20}) 87 | 88 | Ainv = [... 89 | 1, 0, 0, 0, 0, 0; 90 | 0, 0, 1, 0, 0, 0; 91 | 0, 0, 0, 0, 1/2, 0; 92 | -10, 10, -6, -4, -3/2, 1/2; 93 | 15, -15, 8, 7, 3/2, -1; 94 | -6, 6, -3, -3, -1/2, 1/2]; 95 | 96 | % A_yaw = [ 1, 0, 0, 0; 97 | % 1, 1, 1, 1; 98 | % 0, 1, 0, 0; 99 | % 0, 1, 2, 3]; 100 | % 101 | % A_yaw_inv = inv(A_yaw); 102 | 103 | %% Scale derivatives so the boundary conditions are correct despite the time scaling 104 | 105 | % We can scale the rows of A or we can scale the boundary conditions. The nice 106 | % thing about scaling boundary conditions is that we can precompute inv(A). 107 | 108 | % A([3,4],:) = 1 / total_t * A([3,4],:); % Velocities 109 | % 110 | % A([5,6],:) = 1 / total_t^2 * A([5,6],:); % Accelerations 111 | 112 | disp(['New method, condition number:', num2str(cond(A))]); 113 | 114 | bcs([3,4]) = total_t * bcs([3,4]); % Velocities 115 | bcs([5,6]) = total_t^2 * bcs([5,6]); % Accelerations 116 | 117 | x = Ainv * bcs; 118 | 119 | end 120 | 121 | function b = evaluate_poly(coeffs, t) 122 | 123 | n = length(t); 124 | 125 | % Make sure t is in the 3rd dimension 126 | t = reshape(t(:),[1,1,n]); 127 | 128 | l = ones(1,1,n); 129 | o = zeros(1,1,n); 130 | 131 | A = [... 132 | l, t, t.^2, t.^3, t.^4, t.^5; 133 | o, l, 2*t, 3*t.^2, 4*t.^3, 5*t.^4; 134 | o, o, 2*l, 6*t, 12*t.^2, 20*t.^3; 135 | o, o, o, 6*l, 24*t, 60*t.^2]; 136 | 137 | b = zeros(size(A,1),n); 138 | for idx = 1:n 139 | b(:,idx) = A(:,:,idx) * coeffs; 140 | end 141 | 142 | end -------------------------------------------------------------------------------- /trackers/kr_trackers/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | This is a tracker which follows a line from the current position to a given goal position using min jerk trajectory. 6 | 7 | 8 | 9 | 10 | 11 | This is a tracker which follows a line from the current position to a given goal position using a trapezoidal trajectory with distance along the trajectory as the parameter instead of time. 12 | 13 | 14 | 15 | 16 | 17 | This tracker literally does nothing. 18 | 19 | 20 | 21 | 22 | 23 | This tracker commands a desired velocity for the robot. 24 | 25 | 26 | 27 | 28 | 29 | This tracker goes from point A to point B trying to maintain constant velocity with a smooth ramp up. 30 | 31 | 32 | 33 | 34 | 35 | This tracker commands the robot to fly in a circle of a given size and period. 36 | 37 | 38 | 39 | 40 | 41 | This tracker commands the robot to follow a smooth trajectory passing through multiple waypoints. 42 | 43 | 44 | 45 | 46 | 47 | Follows a complex 3D Lissajous tracjectory 48 | 49 | 50 | 51 | 52 | 53 | Follows a sum of two Lissajous trajectories 54 | 55 | 56 | 57 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /trackers/kr_trackers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr_trackers 4 | 1.0.0 5 | kr_trackers 6 | Kartik Mohta 7 | Michael Watterson 8 | Justin Thomas 9 | 10 | BSD 11 | 12 | Kartik Mohta 13 | 14 | catkin 15 | 16 | actionlib 17 | std_msgs 18 | geometry_msgs 19 | nav_msgs 20 | tf 21 | kr_mav_msgs 22 | kr_tracker_msgs 23 | kr_trackers_manager 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /trackers/kr_trackers/scripts/twist_to_velocity_goal.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | from __future__ import print_function 3 | 4 | import rospy 5 | import actionlib 6 | 7 | from geometry_msgs.msg import Twist 8 | #from kr_tracker_msgs.msg import VelocityTrackerAction, VelocityTrackerGoal 9 | from kr_tracker_msgs.msg import VelocityGoal, TrackerStatus 10 | from kr_tracker_msgs.srv import Transition 11 | 12 | class TwistToAction(object): 13 | def __init__(self): 14 | self.ns = '/quadrotor' 15 | 16 | # self.client = actionlib.SimpleActionClient('trackers_manager/velocity_tracker/VelocityTrackerAction', VelocityTrackerAction) 17 | # rospy.loginfo("Waiting for server") 18 | # self.client.wait_for_server() 19 | # rospy.loginfo("Connected!") 20 | self.current_tracker = "" 21 | self.vel_pub = rospy.Publisher("trackers_manager/velocity_tracker/goal", VelocityGoal, queue_size=1) 22 | rospy.Subscriber("cmd_vel", Twist, self.callback, queue_size=1) 23 | rospy.Subscriber("trackers_manager/status", TrackerStatus, self.status_callback, queue_size=1) 24 | 25 | def status_callback(self, data): 26 | self.current_tracker = data.tracker 27 | 28 | def callback(self, data): 29 | 30 | goal = VelocityGoal() 31 | goal.header.stamp = rospy.Time.now() 32 | goal.vx = data.linear.x 33 | goal.vy = data.linear.y 34 | goal.vz = data.linear.z 35 | goal.vyaw = data.angular.z 36 | goal.use_position_gains = True 37 | print(goal) 38 | 39 | self.vel_pub.publish(goal) 40 | 41 | # self.client.send_goal(goal) 42 | 43 | if(self.current_tracker != "kr_trackers/VelocityTracker"): 44 | rospy.wait_for_service('trackers_manager/transition') 45 | try: 46 | transition_tracker = rospy.ServiceProxy('trackers_manager/transition', Transition) 47 | resp1 = transition_tracker('kr_trackers/VelocityTracker') 48 | print(resp1) 49 | except rospy.ServiceException as e: 50 | print("Service call failed: %s"%e) 51 | 52 | def main(): 53 | rospy.init_node('twist_to_velocity_goal') 54 | 55 | tta = TwistToAction() 56 | 57 | rospy.spin() 58 | return 0 59 | 60 | if __name__ == '__main__': 61 | main() 62 | -------------------------------------------------------------------------------- /trackers/kr_trackers/scripts/waypoints_to_action.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | from __future__ import print_function 3 | 4 | import rospy 5 | import actionlib 6 | 7 | from nav_msgs.msg import Path 8 | from kr_tracker_msgs.msg import TrajectoryTrackerAction, TrajectoryTrackerGoal 9 | from kr_tracker_msgs.srv import Transition 10 | 11 | class WpToAction(object): 12 | def __init__(self): 13 | self.client = actionlib.SimpleActionClient('trackers_manager/trajectory_tracker/TrajectoryTracker', TrajectoryTrackerAction) 14 | rospy.loginfo("Waiting for server") 15 | self.client.wait_for_server() 16 | rospy.loginfo("Connected!") 17 | 18 | rospy.Subscriber("waypoints", Path, self.callback, queue_size=1) 19 | 20 | def callback(self, data): 21 | 22 | goal = TrajectoryTrackerGoal() 23 | for dt in data.poses: 24 | goal.waypoints.append(dt.pose) 25 | 26 | print(goal) 27 | 28 | self.client.send_goal(goal) 29 | 30 | rospy.wait_for_service('trackers_manager/transition') 31 | try: 32 | transition_tracker = rospy.ServiceProxy('trackers_manager/transition', Transition) 33 | resp = transition_tracker('kr_trackers/TrajectoryTracker') 34 | print(resp) 35 | except rospy.ServiceException as e: 36 | print("Service call failed: %s"%e) 37 | 38 | def main(): 39 | rospy.init_node('wp_to_action') 40 | 41 | wta = WpToAction() 42 | 43 | rospy.spin() 44 | return 0 45 | 46 | if __name__ == '__main__': 47 | main() 48 | -------------------------------------------------------------------------------- /trackers/kr_trackers/src/initial_conditions.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Class to hold initial conditions for trajectory generation. 3 | * 4 | * Written by Justin Thomas 5 | * 6 | * In most cases we would want to have consecutive trajectories to smoothly 7 | * transition from one to another, so we need to keep the desired command 8 | * continuous. This class stores the last command so as to use it as an initial 9 | * condition for the next trajectory leading to a smooth desired command. 10 | * Previously, we used the current odom of the robot to set the initial 11 | * condition and if the robot had some tracking error, it would lead to a jump 12 | * in the desired, which we want to avoid. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | InitialConditions::InitialConditions() 19 | : pos_(Eigen::Vector3f::Zero()), 20 | vel_(Eigen::Vector3f::Zero()), 21 | acc_(Eigen::Vector3f::Zero()), 22 | jrk_(Eigen::Vector3f::Zero()), 23 | yaw_(0.0f), 24 | yaw_dot_(0.0f), 25 | cmd_valid_(false) 26 | { 27 | } 28 | 29 | void InitialConditions::set_from_cmd(const kr_mav_msgs::PositionCommand::ConstPtr &msg) 30 | { 31 | if(msg == NULL) 32 | { 33 | ROS_WARN("Null PositionCommand recieved. Not setting initial condition."); 34 | return; 35 | } 36 | 37 | pos_ = Eigen::Vector3f(msg->position.x, msg->position.y, msg->position.z); 38 | vel_ = Eigen::Vector3f(msg->velocity.x, msg->velocity.y, msg->velocity.z); 39 | acc_ = Eigen::Vector3f(msg->acceleration.x, msg->acceleration.y, msg->acceleration.z); 40 | jrk_ = Eigen::Vector3f(msg->jerk.x, msg->jerk.y, msg->jerk.z); 41 | yaw_ = msg->yaw; 42 | yaw_dot_ = msg->yaw_dot; 43 | 44 | cmd_valid_ = true; 45 | } 46 | 47 | void InitialConditions::set_from_odom(const nav_msgs::Odometry::ConstPtr &msg) 48 | { 49 | if(!cmd_valid_) 50 | { 51 | pos_ = Eigen::Vector3f(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); 52 | vel_ = Eigen::Vector3f(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z); 53 | acc_ = Eigen::Vector3f(0, 0, 0); 54 | jrk_ = Eigen::Vector3f(0, 0, 0); 55 | yaw_ = tf::getYaw(msg->pose.pose.orientation); 56 | yaw_dot_ = msg->twist.twist.angular.z; // TODO: Should double check which 57 | // frame (body or world) this is in 58 | } 59 | } 60 | 61 | void InitialConditions::reset() 62 | { 63 | cmd_valid_ = false; 64 | } 65 | -------------------------------------------------------------------------------- /trackers/kr_trackers/src/null_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class NullTracker : public kr_trackers_manager::Tracker 6 | { 7 | public: 8 | void Initialize(const ros::NodeHandle &nh); 9 | bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); 10 | void Deactivate(void); 11 | 12 | kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); 13 | uint8_t status() const; 14 | }; 15 | 16 | void NullTracker::Initialize(const ros::NodeHandle &nh) {} 17 | 18 | bool NullTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) 19 | { 20 | return true; 21 | } 22 | 23 | void NullTracker::Deactivate(void) {} 24 | 25 | kr_mav_msgs::PositionCommand::ConstPtr NullTracker::update(const nav_msgs::Odometry::ConstPtr &msg) 26 | { 27 | // Return a null message (will not publish the position command) 28 | return kr_mav_msgs::PositionCommand::Ptr(); 29 | } 30 | 31 | uint8_t NullTracker::status() const 32 | { 33 | return kr_tracker_msgs::TrackerStatus::SUCCEEDED; 34 | } 35 | 36 | #include 37 | PLUGINLIB_EXPORT_CLASS(NullTracker, kr_trackers_manager::Tracker); 38 | -------------------------------------------------------------------------------- /trackers/kr_trackers/src/velocity_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class VelocityTracker : public kr_trackers_manager::Tracker 8 | { 9 | public: 10 | VelocityTracker(void); 11 | 12 | void Initialize(const ros::NodeHandle &nh); 13 | bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); 14 | void Deactivate(void); 15 | 16 | kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); 17 | uint8_t status() const; 18 | 19 | private: 20 | void velocity_cmd_cb(const kr_tracker_msgs::VelocityGoal::ConstPtr &msg); 21 | 22 | ros::Subscriber sub_vel_cmd_, sub_position_vel_cmd_; 23 | kr_mav_msgs::PositionCommand position_cmd_; 24 | bool odom_set_, active_, use_position_gains_; 25 | double last_t_; 26 | double pos_[3], cur_yaw_; 27 | ros::Time last_cmd_time_; 28 | 29 | float timeout_; 30 | }; 31 | 32 | VelocityTracker::VelocityTracker(void) : odom_set_(false), active_(false), use_position_gains_(false), last_t_(0) {} 33 | 34 | void VelocityTracker::Initialize(const ros::NodeHandle &nh) 35 | { 36 | ros::NodeHandle priv_nh(nh, "velocity_tracker"); 37 | priv_nh.param("timeout", timeout_, 0.5f); 38 | 39 | sub_vel_cmd_ = 40 | priv_nh.subscribe("goal", 10, &VelocityTracker::velocity_cmd_cb, this, ros::TransportHints().tcpNoDelay()); 41 | } 42 | 43 | bool VelocityTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) 44 | { 45 | if(cmd) 46 | { 47 | position_cmd_.position = cmd->position; 48 | position_cmd_.yaw = cmd->yaw; 49 | 50 | active_ = true; 51 | } 52 | else if(odom_set_) 53 | { 54 | position_cmd_.position.x = pos_[0]; 55 | position_cmd_.position.y = pos_[1]; 56 | position_cmd_.position.z = pos_[2]; 57 | position_cmd_.yaw = cur_yaw_; 58 | 59 | active_ = true; 60 | } 61 | 62 | return active_; 63 | } 64 | 65 | void VelocityTracker::Deactivate(void) 66 | { 67 | active_ = false; 68 | odom_set_ = false; 69 | last_t_ = 0; 70 | } 71 | 72 | kr_mav_msgs::PositionCommand::ConstPtr VelocityTracker::update(const nav_msgs::Odometry::ConstPtr &msg) 73 | { 74 | pos_[0] = msg->pose.pose.position.x; 75 | pos_[1] = msg->pose.pose.position.y; 76 | pos_[2] = msg->pose.pose.position.z; 77 | cur_yaw_ = tf::getYaw(msg->pose.pose.orientation); 78 | odom_set_ = true; 79 | 80 | if(!active_) 81 | return kr_mav_msgs::PositionCommand::Ptr(); 82 | 83 | if((ros::Time::now() - last_cmd_time_).toSec() > timeout_) 84 | { 85 | // TODO: How much overshoot will this cause at high velocities? 86 | // Ideally ramp down? 87 | position_cmd_.velocity.x = 0.0; 88 | position_cmd_.velocity.y = 0.0; 89 | position_cmd_.velocity.z = 0.0; 90 | position_cmd_.yaw_dot = 0.0; 91 | ROS_WARN_THROTTLE(1, "VelocityTracker is active but timed out"); 92 | 93 | if(use_position_gains_) 94 | position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_NONE; 95 | 96 | position_cmd_.header.stamp = msg->header.stamp; 97 | position_cmd_.header.frame_id = msg->header.frame_id; 98 | last_t_ = 0; 99 | return kr_mav_msgs::PositionCommand::ConstPtr(new kr_mav_msgs::PositionCommand(position_cmd_)); 100 | } 101 | 102 | if(last_t_ == 0) 103 | last_t_ = ros::Time::now().toSec(); 104 | 105 | const double t_now = ros::Time::now().toSec(); 106 | const double dt = t_now - last_t_; 107 | last_t_ = t_now; 108 | 109 | if(use_position_gains_) 110 | { 111 | position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_NONE; 112 | 113 | position_cmd_.position.x = position_cmd_.position.x + dt * position_cmd_.velocity.x; 114 | position_cmd_.position.y = position_cmd_.position.y + dt * position_cmd_.velocity.y; 115 | position_cmd_.position.z = position_cmd_.position.z + dt * position_cmd_.velocity.z; 116 | } 117 | else 118 | { 119 | position_cmd_.kx[0] = 0, position_cmd_.kx[1] = 0, position_cmd_.kx[2] = 0; 120 | position_cmd_.use_msg_gains_flags = kr_mav_msgs::PositionCommand::USE_MSG_GAINS_POSITION_ALL; 121 | 122 | position_cmd_.position.x = pos_[0]; 123 | position_cmd_.position.y = pos_[1]; 124 | position_cmd_.position.z = pos_[2]; 125 | } 126 | position_cmd_.yaw = position_cmd_.yaw + dt * position_cmd_.yaw_dot; 127 | 128 | position_cmd_.header.stamp = msg->header.stamp; 129 | position_cmd_.header.frame_id = msg->header.frame_id; 130 | 131 | return kr_mav_msgs::PositionCommand::ConstPtr(new kr_mav_msgs::PositionCommand(position_cmd_)); 132 | } 133 | 134 | void VelocityTracker::velocity_cmd_cb(const kr_tracker_msgs::VelocityGoal::ConstPtr &msg) 135 | { 136 | // ROS_INFO("VelocityTracker goal (%2.2f, %2.2f, %2.2f, %2.2f)", msg->vx, msg->vy, msg->vz, msg->vyaw); 137 | position_cmd_.velocity.x = msg->vx; 138 | position_cmd_.velocity.y = msg->vy; 139 | position_cmd_.velocity.z = msg->vz; 140 | position_cmd_.yaw_dot = msg->vyaw; 141 | 142 | use_position_gains_ = msg->use_position_gains; 143 | 144 | last_cmd_time_ = ros::Time::now(); 145 | } 146 | 147 | uint8_t VelocityTracker::status() const 148 | { 149 | return active_ ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : 150 | static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); 151 | } 152 | 153 | #include 154 | PLUGINLIB_EXPORT_CLASS(VelocityTracker, kr_trackers_manager::Tracker) 155 | -------------------------------------------------------------------------------- /trackers/kr_trackers_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kr_trackers_manager) 3 | 4 | # set default build type 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 7 | endif() 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_CXX_EXTENSIONS OFF) 12 | add_compile_options(-Wall) 13 | 14 | find_package( 15 | catkin REQUIRED 16 | COMPONENTS roscpp 17 | nodelet 18 | pluginlib 19 | nav_msgs 20 | kr_mav_msgs 21 | kr_tracker_msgs) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS 25 | include 26 | LIBRARIES 27 | ${PROJECT_NAME} 28 | CATKIN_DEPENDS 29 | roscpp 30 | nodelet 31 | pluginlib 32 | kr_mav_msgs 33 | nav_msgs 34 | kr_tracker_msgs) 35 | 36 | add_library(${PROJECT_NAME} src/trackers_manager.cpp) 37 | target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) 38 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) 39 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 40 | 41 | install( 42 | TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 46 | 47 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 48 | install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 49 | -------------------------------------------------------------------------------- /trackers/kr_trackers_manager/include/kr_trackers_manager/Tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef TRACKERS_MANAGER_TRACKER_H_ 2 | #define TRACKERS_MANAGER_TRACKER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace kr_trackers_manager 9 | { 10 | class Tracker 11 | { 12 | public: 13 | virtual ~Tracker(void) {} 14 | 15 | /** 16 | * @brief Initialize the tracker. Should be used to get the params, construct the publishers and subscribers. 17 | * 18 | * @param nh The NodeHandle with the kr_trackers_manager's namespace, can be used to read common params such as gains. 19 | */ 20 | virtual void Initialize(const ros::NodeHandle &nh) = 0; 21 | 22 | /** 23 | * @brief Activate the tracker. This indicates that the tracker should get ready to publish commands. 24 | * 25 | * @param cmd The last PositionCommand that was published, can be used to maintain continuity of commands when 26 | * switching trackers. 27 | * 28 | * @return Should return true if the tracker is ready to publish commands, else return false. 29 | */ 30 | virtual bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) = 0; 31 | 32 | /** 33 | * @brief Deactivate the tracker. This is called when the kr_trackers_manager switches to another tracker. 34 | */ 35 | virtual void Deactivate(void) = 0; 36 | 37 | /** 38 | * @brief Get the current command output from the tracker. 39 | * Note that this function is still called even if the tracker has not been activated. This is for cases when the 40 | * tracker would want to use the previous robot odometry to compute current commands. 41 | * 42 | * @param msg The current odometry message which should be used by the tracker to generate the command. 43 | * 44 | * @return The PositionCommand message which would be published. If an uninitialized ConstPtr is returned, then no 45 | * PositionCommand message would be published. 46 | */ 47 | virtual kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg) = 0; 48 | 49 | /** 50 | * @brief Get status of the tracker. Only called when the tracker has been activated. 51 | * 52 | * @return The tracker status (see the options in the TrackerStatus message). 53 | */ 54 | virtual uint8_t status() const = 0; 55 | }; 56 | 57 | } // namespace kr_trackers_manager 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /trackers/kr_trackers_manager/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /trackers/kr_trackers_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr_trackers_manager 4 | 1.0.0 5 | kr_trackers_manager 6 | Kartik Mohta 7 | Michael Watterson 8 | Justin Thomas 9 | 10 | BSD 11 | 12 | Kartik Mohta 13 | 14 | catkin 15 | roscpp 16 | nodelet 17 | pluginlib 18 | kr_mav_msgs 19 | nav_msgs 20 | kr_tracker_msgs 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /trackers/kr_trackers_manager/src/trackers_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class TrackersManager : public nodelet::Nodelet 10 | { 11 | public: 12 | TrackersManager(void); 13 | ~TrackersManager(void); 14 | 15 | void onInit(void); 16 | 17 | private: 18 | void odom_callback(const nav_msgs::Odometry::ConstPtr &msg); 19 | bool transition_callback(kr_tracker_msgs::Transition::Request &req, kr_tracker_msgs::Transition::Response &res); 20 | 21 | ros::Subscriber sub_odom_; 22 | ros::Publisher pub_cmd_, pub_status_; 23 | ros::ServiceServer srv_tracker_; 24 | pluginlib::ClassLoader tracker_loader_; 25 | kr_trackers_manager::Tracker *active_tracker_; 26 | std::map tracker_map_; 27 | kr_mav_msgs::PositionCommand::ConstPtr cmd_; 28 | }; 29 | 30 | TrackersManager::TrackersManager(void) 31 | : tracker_loader_("kr_trackers_manager", "kr_trackers_manager::Tracker"), active_tracker_(NULL) 32 | { 33 | } 34 | 35 | TrackersManager::~TrackersManager(void) 36 | { 37 | for(std::map::iterator it = tracker_map_.begin(); 38 | it != tracker_map_.end(); it++) 39 | { 40 | delete it->second; 41 | #if ROS_VERSION_MINIMUM(1, 8, 0) 42 | try 43 | { 44 | tracker_loader_.unloadLibraryForClass(it->first); 45 | } 46 | catch(pluginlib::LibraryUnloadException &e) 47 | { 48 | NODELET_ERROR_STREAM("Could not unload library for the tracker " << it->first << ": " << e.what()); 49 | } 50 | #endif 51 | } 52 | } 53 | 54 | void TrackersManager::onInit(void) 55 | { 56 | ros::NodeHandle priv_nh(getPrivateNodeHandle()); 57 | 58 | XmlRpc::XmlRpcValue tracker_list; 59 | priv_nh.getParam("trackers", tracker_list); 60 | ROS_ASSERT(tracker_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 61 | for(int i = 0; i < tracker_list.size(); i++) 62 | { 63 | ROS_ASSERT(tracker_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); 64 | const std::string tracker_name = static_cast(tracker_list[i]); 65 | try 66 | { 67 | #if ROS_VERSION_MINIMUM(1, 8, 0) 68 | kr_trackers_manager::Tracker *c = tracker_loader_.createUnmanagedInstance(tracker_name); 69 | #else 70 | kr_trackers_manager::Tracker *c = tracker_loader_.createClassInstance(tracker_name); 71 | #endif 72 | c->Initialize(priv_nh); 73 | tracker_map_.insert(std::make_pair(tracker_name, c)); 74 | } 75 | catch(pluginlib::LibraryLoadException &e) 76 | { 77 | NODELET_ERROR_STREAM("Could not load library for the tracker " << tracker_name << ": " << e.what()); 78 | } 79 | catch(pluginlib::CreateClassException &e) 80 | { 81 | NODELET_ERROR_STREAM("Could not create an instance of the tracker " << tracker_name << ": " << e.what()); 82 | } 83 | } 84 | 85 | pub_cmd_ = priv_nh.advertise("cmd", 10); 86 | pub_status_ = priv_nh.advertise("status", 10); 87 | 88 | sub_odom_ = priv_nh.subscribe("odom", 10, &TrackersManager::odom_callback, this, ros::TransportHints().tcpNoDelay()); 89 | 90 | srv_tracker_ = priv_nh.advertiseService("transition", &TrackersManager::transition_callback, this); 91 | } 92 | 93 | void TrackersManager::odom_callback(const nav_msgs::Odometry::ConstPtr &msg) 94 | { 95 | std::map::iterator it; 96 | for(it = tracker_map_.begin(); it != tracker_map_.end(); it++) 97 | { 98 | if(it->second == active_tracker_) 99 | { 100 | cmd_ = it->second->update(msg); 101 | if(cmd_ != NULL) 102 | pub_cmd_.publish(cmd_); 103 | 104 | kr_tracker_msgs::TrackerStatus::Ptr status_msg(new kr_tracker_msgs::TrackerStatus); 105 | status_msg->header.stamp = msg->header.stamp; 106 | status_msg->tracker = it->first; 107 | status_msg->status = it->second->status(); 108 | pub_status_.publish(status_msg); 109 | } 110 | else 111 | { 112 | it->second->update(msg); 113 | } 114 | } 115 | } 116 | 117 | bool TrackersManager::transition_callback(kr_tracker_msgs::Transition::Request &req, 118 | kr_tracker_msgs::Transition::Response &res) 119 | { 120 | const std::map::iterator it = tracker_map_.find(req.tracker); 121 | if(it == tracker_map_.end()) 122 | { 123 | res.success = false; 124 | res.message = std::string("Cannot find tracker ") + req.tracker + std::string(", cannot transition"); 125 | NODELET_WARN_STREAM(res.message); 126 | return true; 127 | } 128 | if(active_tracker_ == it->second) 129 | { 130 | res.success = true; 131 | res.message = std::string("Tracker ") + req.tracker + std::string(" already active"); 132 | NODELET_INFO_STREAM(res.message); 133 | return true; 134 | } 135 | 136 | if(!it->second->Activate(cmd_)) 137 | { 138 | res.success = false; 139 | res.message = std::string("Failed to activate tracker ") + req.tracker + std::string(", cannot transition"); 140 | NODELET_WARN_STREAM(res.message); 141 | return true; 142 | } 143 | 144 | if(active_tracker_ != NULL) 145 | { 146 | active_tracker_->Deactivate(); 147 | } 148 | 149 | active_tracker_ = it->second; 150 | res.success = true; 151 | res.message = std::string("Successfully activated tracker ") + req.tracker; 152 | return true; 153 | } 154 | 155 | #include 156 | PLUGINLIB_EXPORT_CLASS(TrackersManager, nodelet::Nodelet); 157 | --------------------------------------------------------------------------------