├── .gitignore ├── README.md ├── Utils ├── odom_visualization │ ├── CMakeLists.txt │ ├── Makefile │ ├── mainpage.dox │ ├── meshes │ │ └── hummingbird.mesh │ ├── package.xml │ └── src │ │ ├── CameraPoseVisualization.cpp │ │ ├── CameraPoseVisualization.h │ │ └── odom_visualization.cpp ├── pose_utils │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── pose_utils.h │ ├── package.xml │ └── src │ │ └── pose_utils.cpp ├── quadrotor_msgs │ ├── CMakeLists.txt │ ├── cmake │ │ └── FindEigen3.cmake │ ├── include │ │ └── quadrotor_msgs │ │ │ ├── comm_types.h │ │ │ ├── decode_msgs.h │ │ │ └── encode_msgs.h │ ├── msg │ │ ├── AuxCommand.msg │ │ ├── Corrections.msg │ │ ├── Gains.msg │ │ ├── Odometry.msg │ │ ├── OutputData.msg │ │ ├── PPROutputData.msg │ │ ├── PolynomialMatrix.msg │ │ ├── PolynomialTraj.msg │ │ ├── PolynomialTrajectory.msg │ │ ├── PositionCommand.msg │ │ ├── Px4ctrlDebug.msg │ │ ├── SO3Command.msg │ │ ├── Serial.msg │ │ ├── StatusData.msg │ │ ├── TRPYCommand.msg │ │ ├── TakeoffLand.msg │ │ └── Trajectory.msg │ ├── package.xml │ └── src │ │ ├── decode_msgs.cpp │ │ ├── encode_msgs.cpp │ │ └── quadrotor_msgs │ │ ├── __init__.py │ │ └── msg │ │ ├── _AuxCommand.py │ │ ├── _Corrections.py │ │ ├── _Gains.py │ │ ├── _OutputData.py │ │ ├── _PPROutputData.py │ │ ├── _PositionCommand.py │ │ ├── _SO3Command.py │ │ ├── _Serial.py │ │ ├── _StatusData.py │ │ ├── _TRPYCommand.py │ │ └── __init__.py ├── rviz_plugins │ ├── CMakeLists.txt │ ├── config │ │ └── rviz_config.rviz │ ├── package.xml │ ├── plugin_description.xml │ └── src │ │ ├── goal_tool.cpp │ │ ├── goal_tool.h │ │ ├── pose_tool.cpp │ │ └── pose_tool.h └── uav_utils │ ├── CMakeLists.txt │ ├── include │ └── uav_utils │ │ ├── converters.h │ │ ├── geometry_utils.h │ │ └── utils.h │ ├── package.xml │ ├── scripts │ ├── odom_to_euler.py │ ├── send_odom.py │ ├── tf_assist.py │ └── topic_statistics.py │ └── src │ └── uav_utils_test.cpp ├── controller └── payload_mpc_controller │ ├── CMakeLists.txt │ ├── config │ ├── model.yaml │ └── mpc.yaml │ ├── externals │ └── qpoases │ │ ├── EXAMPLES │ │ ├── example1.cpp │ │ └── example1b.cpp │ │ ├── INCLUDE │ │ ├── Bounds.hpp │ │ ├── Constants.hpp │ │ ├── Constraints.hpp │ │ ├── CyclingManager.hpp │ │ ├── EXTRAS │ │ │ └── SolutionAnalysis.hpp │ │ ├── Indexlist.hpp │ │ ├── MessageHandling.hpp │ │ ├── QProblem.hpp │ │ ├── QProblemB.hpp │ │ ├── SubjectTo.hpp │ │ ├── Types.hpp │ │ └── Utils.hpp │ │ ├── LICENSE.txt │ │ ├── README.txt │ │ ├── SRC │ │ ├── Bounds.cpp │ │ ├── Bounds.ipp │ │ ├── Constraints.cpp │ │ ├── Constraints.ipp │ │ ├── CyclingManager.cpp │ │ ├── CyclingManager.ipp │ │ ├── EXTRAS │ │ │ └── SolutionAnalysis.cpp │ │ ├── Indexlist.cpp │ │ ├── Indexlist.ipp │ │ ├── MessageHandling.cpp │ │ ├── MessageHandling.ipp │ │ ├── QProblem.cpp │ │ ├── QProblem.ipp │ │ ├── QProblemB.cpp │ │ ├── QProblemB.ipp │ │ ├── SubjectTo.cpp │ │ ├── SubjectTo.ipp │ │ ├── Utils.cpp │ │ └── Utils.ipp │ │ └── VERSIONS.txt │ ├── include │ └── payload_mpc_controller │ │ ├── flatness.h │ │ ├── force_estimator.hpp │ │ ├── lbfgs.hpp │ │ ├── lowpassfilter2p.h │ │ ├── mpc_controller.h │ │ ├── mpc_fsm.h │ │ ├── mpc_input.h │ │ ├── mpc_params.h │ │ ├── mpc_wrapper.h │ │ ├── multi_optimization_based_force_estimator.hpp │ │ ├── multi_optimization_based_force_estimator_min_length.hpp │ │ ├── optimization_based_force_estimator_lbfgs.hpp │ │ └── polynomial_trajectory.h │ ├── model │ ├── CMakeLists.txt │ ├── FindACADO.cmake │ ├── Makefile │ ├── README.md │ ├── Yaml.cpp │ ├── Yaml.hpp │ ├── quadrotor_payload_mpc │ │ ├── acado_auxiliary_functions.c │ │ ├── acado_auxiliary_functions.h │ │ ├── acado_common.h │ │ ├── acado_integrator.c │ │ ├── acado_qpoases_interface.cpp │ │ ├── acado_qpoases_interface.hpp │ │ └── acado_solver.c │ ├── quadrotor_payload_mpc_hybird.cpp │ └── quadrotor_payload_mpc_with_ext_force.cpp │ ├── package.xml │ └── src │ ├── mpc_controller.cpp │ ├── mpc_controller_node.cpp │ ├── mpc_fsm.cpp │ ├── mpc_input.cpp │ └── mpc_wrapper.cpp ├── imgs ├── cover.png ├── simulation.gif └── system.png ├── planner ├── path_searching │ ├── CMakeLists.txt │ ├── include │ │ └── path_searching │ │ │ ├── dyn_a_star.h │ │ │ └── kinodynamic_astar.h │ ├── package.xml │ └── src │ │ ├── dyn_a_star.cpp │ │ └── kinodynamic_astar.cpp ├── plan_env │ ├── CMakeLists.txt │ ├── include │ │ └── plan_env │ │ │ ├── grid_map.h │ │ │ └── raycast.h │ ├── package.xml │ └── src │ │ ├── grid_map.cpp │ │ └── raycast.cpp ├── plan_manage │ ├── CMakeLists.txt │ ├── config │ │ └── planning_params.yaml │ ├── include │ │ └── plan_manage │ │ │ ├── backward.hpp │ │ │ ├── planner_manager.h │ │ │ ├── planning_visualization.h │ │ │ └── replan_fsm.h │ ├── launch │ │ ├── planning_node_params.xml │ │ ├── replan.launch │ │ ├── rviz.launch │ │ ├── sim_vis.rviz │ │ ├── simple_run.launch │ │ └── simulator.xml │ ├── package.xml │ └── src │ │ ├── payload_planner_node.cpp │ │ ├── planner_manager.cpp │ │ ├── planning_visualization.cpp │ │ └── replan_fsm.cpp ├── traj_opt │ ├── CMakeLists.txt │ ├── include │ │ └── optimizer │ │ │ ├── flatness.hpp │ │ │ ├── lbfgs.hpp │ │ │ ├── plan_container.hpp │ │ │ ├── poly_traj_optimizer.h │ │ │ ├── poly_traj_utils.hpp │ │ │ └── root_finder.hpp │ ├── package.xml │ └── src │ │ └── poly_traj_optimizer.cpp └── traj_utils │ ├── CMakeLists.txt │ ├── msg │ └── DataDisp.msg │ ├── package.xml │ └── src │ └── main.cpp └── uav_simulator ├── local_sensing ├── CMakeLists.txt ├── CMakeModules │ ├── FindCUDA.cmake │ ├── FindCUDA │ │ ├── make2cmake.cmake │ │ ├── parse_cubin.cmake │ │ └── run_nvcc.cmake │ └── FindEigen.cmake ├── cfg │ └── local_sensing_node.cfg ├── package.xml ├── params │ └── camera.yaml └── src │ ├── AlignError.h │ ├── ceres_extensions.h │ ├── csv_convert.py │ ├── cuda_exception.cuh │ ├── depth_render.cu │ ├── depth_render.cuh │ ├── device_image.cuh │ ├── empty.cpp │ ├── empty.h │ ├── euroc.cpp │ ├── helper_math.h │ ├── pcl_render_node.cpp │ └── pointcloud_render_node.cpp ├── map_generator ├── CMakeLists.txt ├── include │ └── map_generator │ │ └── map_generator.hpp ├── package.xml ├── payload_replan.launch └── src │ ├── map_generator.cpp │ └── random_forest_sensing.cpp ├── mockamap ├── CMakeLists.txt ├── README.md ├── config │ └── rviz.rviz ├── include │ ├── maps.hpp │ └── perlinnoise.hpp ├── launch │ ├── maze2d.launch │ ├── maze3d.launch │ ├── mockamap.launch │ ├── perlin3d.launch │ └── post2d.launch ├── package.xml └── src │ ├── .clang-format │ ├── ces_randommap.cpp │ ├── maps.cpp │ ├── mockamap.cpp │ └── perlinnoise.cpp ├── so3_quadrotor ├── CMakeLists.txt ├── include │ └── so3_quadrotor │ │ ├── geometry_utils.hpp │ │ └── quadrotor_dynamics.hpp ├── nodelet_plugin.xml ├── package.xml └── src │ └── so3_quadrotor_nodelet.cpp └── uav_simulator ├── CMakeLists.txt ├── config ├── local_sensing.yaml ├── mockamap.yaml ├── rviz.rviz └── so3_quadrotor.yaml ├── launch └── uav_simulator.launch └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | generating_corridor/* 2 | CMakeCache.txt 3 | CMakeFiles 4 | cmake_install.cmake 5 | quadrotor_payload_mpc_with_ext_force_codegen 6 | *.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AutoTrans: A Complete Planning and Control Framework for Autonomous UAV Payload Transportation 2 | 3 | [![AutoTrans](imgs/cover.png)](https://youtu.be/X9g-ivBqy5g) 4 | 5 | **Video**: [YouTube](https://youtu.be/X9g-ivBqy5g) | [Bilibili](https://www.bilibili.com/video/BV1aM4y1a7c9) 6 | 7 | ## Introduction 8 | **AutoTrans** presents a real-time and robust planning and control framework for quadrotor with payload. It can generate the dynamic feasible trajectory considering the time-varying shape and deal with the disturbance while executing trajectory. 9 | 10 | ![System](imgs/system.png) 11 | 12 | This repo includes a real-time motion planning, a disturbance-aware NMPC and a simple simulation for a quadrotor with a suspended payload. 13 | 14 | 15 | **Authors**: Haojia Li, Haokun Wang, [Chen Feng](https://chen-albert-feng.github.io/AlbertFeng.github.io/), [Fei Gao*](http://zju-fast.com/fei-gao/), [Boyu Zhou*](http://sysu-star.com/), and [Shaojie Shen](https://uav.hkust.edu.hk/group/). 16 | 17 | **Institutions**: [HKUST Aerial Robotics Group](https://uav.hkust.edu.hk/), [SYSU STAR Group](http://sysu-star.com/), and [ZJU FAST Lab](http://zju-fast.com/). 18 | 19 | **Paper**: [AutoTrans: A Complete Planning and Control Framework for Autonomous UAV Payload Transportation](https://doi.org/10.1109/LRA.2023.3313010), IEEE Robotics and Automation Letters (RA-L), 2023. [IEEE Spectrum](https://spectrum.ieee.org/video-friday-reflex-grasping) 20 | 21 | ```bibtex 22 | @article{li2023autotrans, 23 | title={AutoTrans: A Complete Planning and Control Framework for Autonomous UAV Payload Transportation}, 24 | author={Li, Haojia and Wang, Haokun and Feng, Chen and Gao, Fei and Zhou, Boyu and Shen, Shaojie}, 25 | journal={IEEE Robotics and Automation Letters}, 26 | year={2023}, 27 | volume={8}, 28 | number={10}, 29 | pages={6859-6866}, 30 | doi={10.1109/LRA.2023.3313010}} 31 | ``` 32 | 33 | If you find this work useful or interesting, please kindly give us a star ⭐, thanks!😀 34 | 35 | ## Setup 36 | 37 | Compiling tests passed on Ubuntu 20.04/18.04 with ROS installed. 38 | ### Prerequisites 39 | - [ROS](http://wiki.ros.org/ROS/Installation) (tested with Noetic) 40 | 41 | ```bash 42 | sudo apt install ros-"${ROS_DISTRO}"-mavros-msgs 43 | ``` 44 | 45 | ### Compiling and Running 46 | ```bash 47 | # Compile the code 48 | mkdir -p autotrans_ws/src 49 | cd autotrans_ws/src 50 | git clone https://github.com/HKUST-Aerial-Robotics/AutoTrans 51 | cd .. 52 | catkin_make -DCMAKE_BUILD_TYPE=Release 53 | 54 | # Run the simulation with planner and controller 55 | source devel/setup.bash 56 | roslaunch payload_planner simple_run.launch 57 | ``` 58 | You should see the simulation in rviz. You can use the `2D Nav Goal` to set the goal position. The red arrow on the quadrotor represents the estimated disturbance force (like rotor drag). 59 | 60 | ![Simulation](imgs/simulation.gif) 61 | 62 | ### Tip: Boost Your Computer 63 | This can make the MPC solve faster in each iteration. 64 | ```bash 65 | sudo apt install cpufrequtils 66 | sudo cpufreq-set -g performance 67 | ``` 68 | 69 | 70 | 71 | ## Acknowledgements 72 | We use [MINCO](https://github.com/ZJU-FAST-Lab/GCOPTER) as our trajectory representation. 73 | 74 | We borrow the framework from [Fast-Planner](https://github.com/HKUST-Aerial-Robotics/Fast-Planner) and [Ego-Planner-v2](https://github.com/ZJU-FAST-Lab/EGO-Planner-v2) 75 | 76 | Thanks to [rpg_mpc](https://github.com/uzh-rpg/rpg_mpc) for a good reference. 77 | 78 | ## Maintaince 79 | For any technical issues, please contact Haojia Li(hlied@connect.ust.hk). -------------------------------------------------------------------------------- /Utils/odom_visualization/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /Utils/odom_visualization/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b odom_visualization is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /Utils/odom_visualization/meshes/hummingbird.mesh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/Utils/odom_visualization/meshes/hummingbird.mesh -------------------------------------------------------------------------------- /Utils/odom_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 0.0.0 3 | odom_visualization 4 | 5 | 6 | odom_visualization 7 | 8 | 9 | Shaojie Shen 10 | BSD 11 | http://ros.org/wiki/odom_visualization 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | nav_msgs 18 | visualization_msgs 19 | tf 20 | pose_utils 21 | 22 | roscpp 23 | sensor_msgs 24 | nav_msgs 25 | visualization_msgs 26 | tf 27 | pose_utils 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Utils/odom_visualization/src/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class CameraPoseVisualization { 20 | public: 21 | std::string m_marker_ns; 22 | 23 | CameraPoseVisualization(float r, float g, float b, float a); 24 | 25 | void setImageBoundaryColor(float r, float g, float b, float a=1.0); 26 | void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0); 27 | void setScale(double s); 28 | void setLineWidth(double width); 29 | 30 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 31 | void reset(); 32 | 33 | void publish_by(ros::Publisher& pub, const std_msgs::Header& header); 34 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 35 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 36 | private: 37 | std::vector m_markers; 38 | std_msgs::ColorRGBA m_image_boundary_color; 39 | std_msgs::ColorRGBA m_optical_center_connector_color; 40 | double m_scale; 41 | double m_line_width; 42 | 43 | static const Eigen::Vector3d imlt; 44 | static const Eigen::Vector3d imlb; 45 | static const Eigen::Vector3d imrt; 46 | static const Eigen::Vector3d imrb; 47 | static const Eigen::Vector3d oc ; 48 | static const Eigen::Vector3d lt0 ; 49 | static const Eigen::Vector3d lt1 ; 50 | static const Eigen::Vector3d lt2 ; 51 | }; 52 | -------------------------------------------------------------------------------- /Utils/pose_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pose_utils) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-O3 -Wall -g") 6 | ADD_COMPILE_OPTIONS(-std=c++11 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | #armadillo 11 | roscpp 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | LIBRARIES pose_utils 17 | # CATKIN_DEPENDS geometry_msgs nav_msgs 18 | # DEPENDS system_lib 19 | ) 20 | 21 | find_package(Armadillo REQUIRED) 22 | 23 | include_directories( 24 | ${catkin_INCLUDE_DIRS} 25 | ${ARMADILLO_INCLUDE_DIRS} 26 | include 27 | ) 28 | 29 | add_library(pose_utils 30 | ${ARMADILLO_LIBRARIES} 31 | src/pose_utils.cpp) 32 | -------------------------------------------------------------------------------- /Utils/pose_utils/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /Utils/pose_utils/include/pose_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef POSE_UTILS_H 2 | #define POSE_UTILS_H 3 | 4 | #include 5 | #include "armadillo" 6 | 7 | #define PI 3.14159265359 8 | #define NUM_INF 999999.9 9 | 10 | using namespace arma; 11 | using namespace std; 12 | 13 | // Rotation --------------------- 14 | mat ypr_to_R(const colvec& ypr); 15 | 16 | mat yaw_to_R(double yaw); 17 | 18 | colvec R_to_ypr(const mat& R); 19 | 20 | mat quaternion_to_R(const colvec& q); 21 | 22 | colvec R_to_quaternion(const mat& R); 23 | 24 | colvec quaternion_mul(const colvec& q1, const colvec& q2); 25 | 26 | colvec quaternion_inv(const colvec& q); 27 | 28 | // General Pose Update ---------- 29 | colvec pose_update(const colvec& X1, const colvec& X2); 30 | 31 | colvec pose_inverse(const colvec& X); 32 | 33 | colvec pose_update_2d(const colvec& X1, const colvec& X2); 34 | 35 | colvec pose_inverse_2d(const colvec& X); 36 | 37 | // For Pose EKF ----------------- 38 | mat Jplus1(const colvec& X1, const colvec& X2); 39 | 40 | mat Jplus2(const colvec& X1, const colvec& X2); 41 | 42 | // For IMU EKF ------------------ 43 | colvec state_update(const colvec& X, const colvec& U, double dt); 44 | 45 | mat jacobianF(const colvec& X, const colvec& U, double dt); 46 | 47 | mat jacobianU(const colvec& X, const colvec& U, double dt); 48 | 49 | colvec state_measure(const colvec& X); 50 | 51 | mat jacobianH(); 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /Utils/pose_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pose_utils 3 | 0.0.0 4 | pose_utils 5 | Shaojie Shen 6 | BSD 7 | catkin 8 | roscpp 9 | roscpp 10 | 11 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #-cmake_minimum_required(VERSION 2.4.6) 2 | #-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #-rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | #-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | #-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #-\rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | #-rosbuild_add_library(encode_msgs src/encode_msgs.cpp) 33 | #-rosbuild_add_link_flags(encode_msgs -Wl,--as-needed) 34 | 35 | #list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 36 | #find_package(Eigen3 REQUIRED) 37 | 38 | #include_directories(${EIGEN3_INCLUDE_DIR}) 39 | #-rosbuild_add_library(decode_msgs src/decode_msgs.cpp) 40 | #-rosbuild_add_link_flags(decode_msgs -Wl,--as-needed) 41 | 42 | #------------------------------------------------------------------ 43 | cmake_minimum_required(VERSION 2.8.3) 44 | project(quadrotor_msgs) 45 | 46 | find_package(catkin REQUIRED COMPONENTS 47 | #armadillo 48 | roscpp 49 | nav_msgs 50 | geometry_msgs 51 | message_generation 52 | ) 53 | 54 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 55 | 56 | include_directories( 57 | ${catkin_INCLUDE_DIRS} 58 | include 59 | ) 60 | 61 | 62 | add_message_files( 63 | FILES 64 | AuxCommand.msg 65 | Corrections.msg 66 | Gains.msg 67 | OutputData.msg 68 | PositionCommand.msg 69 | PPROutputData.msg 70 | Serial.msg 71 | SO3Command.msg 72 | StatusData.msg 73 | TRPYCommand.msg 74 | Odometry.msg 75 | PolynomialTrajectory.msg 76 | PolynomialTraj.msg 77 | PolynomialMatrix.msg 78 | TakeoffLand.msg 79 | Px4ctrlDebug.msg 80 | 81 | ) 82 | 83 | generate_messages( 84 | DEPENDENCIES 85 | geometry_msgs 86 | nav_msgs 87 | ) 88 | 89 | catkin_package( 90 | #INCLUDE_DIRS include 91 | LIBRARIES encode_msgs decode_msgs 92 | #CATKIN_DEPENDS geometry_msgs nav_msgs 93 | #DEPENDS system_lib 94 | CATKIN_DEPENDS message_runtime 95 | ) 96 | 97 | # add_executable(test_exe src/test_exe.cpp) 98 | add_library(decode_msgs src/decode_msgs.cpp) 99 | add_library(encode_msgs src/encode_msgs.cpp) 100 | 101 | # add_dependencies(test_exe quadrotor_msgs_generate_messages_cpp) 102 | add_dependencies(encode_msgs quadrotor_msgs_generate_messages_cpp) 103 | add_dependencies(decode_msgs quadrotor_msgs_generate_messages_cpp) 104 | 105 | find_package(Eigen3 REQUIRED) 106 | include_directories(${EIGEN3_INCLUDE_DIR}) 107 | 108 | 109 | # target_link_libraries(test_exe 110 | # decode_msgs 111 | # encode_msgs 112 | # ) 113 | 114 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_COMM_TYPES_H__ 2 | #define __QUADROTOR_MSGS_COMM_TYPES_H__ 3 | 4 | #define TYPE_SO3_CMD 's' 5 | struct SO3_CMD_INPUT 6 | { 7 | // Scaling factors when decoding 8 | int16_t force[3]; // /500 9 | int8_t des_qx, des_qy, des_qz, des_qw; // /125 10 | uint8_t kR[3]; // /50 11 | uint8_t kOm[3]; // /100 12 | int16_t cur_yaw; // /1e4 13 | int16_t kf_correction; // /1e11; 14 | uint8_t angle_corrections[2]; // roll,pitch /2500 15 | uint8_t enable_motors:1; 16 | uint8_t use_external_yaw:1; 17 | uint8_t seq; 18 | }; 19 | 20 | #define TYPE_STATUS_DATA 'c' 21 | struct STATUS_DATA 22 | { 23 | uint16_t loop_rate; 24 | uint16_t voltage; 25 | uint8_t seq; 26 | }; 27 | 28 | #define TYPE_OUTPUT_DATA 'd' 29 | struct OUTPUT_DATA 30 | { 31 | uint16_t loop_rate; 32 | uint16_t voltage; 33 | int16_t roll, pitch, yaw; 34 | int16_t ang_vel[3]; 35 | int16_t acc[3]; 36 | int16_t dheight; 37 | int32_t height; 38 | int16_t mag[3]; 39 | uint8_t radio[8]; 40 | //uint8_t rpm[4]; 41 | uint8_t seq; 42 | }; 43 | 44 | #define TYPE_TRPY_CMD 'p' 45 | struct TRPY_CMD 46 | { 47 | int16_t thrust; 48 | int16_t roll; 49 | int16_t pitch; 50 | int16_t yaw; 51 | int16_t current_yaw; 52 | uint8_t enable_motors:1; 53 | uint8_t use_external_yaw:1; 54 | }; 55 | 56 | #define TYPE_PPR_OUTPUT_DATA 't' 57 | struct PPR_OUTPUT_DATA 58 | { 59 | uint16_t time; 60 | int16_t des_thrust; 61 | int16_t des_roll; 62 | int16_t des_pitch; 63 | int16_t des_yaw; 64 | int16_t est_roll; 65 | int16_t est_pitch; 66 | int16_t est_yaw; 67 | int16_t est_angvel_x; 68 | int16_t est_angvel_y; 69 | int16_t est_angvel_z; 70 | int16_t est_acc_x; 71 | int16_t est_acc_y; 72 | int16_t est_acc_z; 73 | uint16_t pwm1; 74 | uint16_t pwm2; 75 | uint16_t pwm3; 76 | uint16_t pwm4; 77 | }; 78 | 79 | #define TYPE_PPR_GAINS 'g' 80 | struct PPR_GAINS 81 | { 82 | int16_t Kp; 83 | int16_t Kd; 84 | int16_t Kp_yaw; 85 | int16_t Kd_yaw; 86 | }; 87 | #endif 88 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | bool decodeOutputData(const std::vector &data, 14 | quadrotor_msgs::OutputData &output); 15 | 16 | bool decodeStatusData(const std::vector &data, 17 | quadrotor_msgs::StatusData &status); 18 | 19 | bool decodePPROutputData(const std::vector &data, 20 | quadrotor_msgs::PPROutputData &output); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 14 | std::vector &output); 15 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 16 | std::vector &output); 17 | 18 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 19 | std::vector &output); 20 | } 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /Utils/quadrotor_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 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Corrections.msg: -------------------------------------------------------------------------------- 1 | float64 kf_correction 2 | float64[2] angle_corrections 3 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Gains.msg: -------------------------------------------------------------------------------- 1 | float64 Kp 2 | float64 Kd 3 | float64 Kp_yaw 4 | float64 Kd_yaw 5 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Odometry.msg: -------------------------------------------------------------------------------- 1 | uint8 STATUS_ODOM_VALID=0 2 | uint8 STATUS_ODOM_INVALID=1 3 | uint8 STATUS_ODOM_LOOPCLOSURE=2 4 | 5 | nav_msgs/Odometry curodom 6 | nav_msgs/Odometry kfodom 7 | uint32 kfid 8 | uint8 status 9 | -------------------------------------------------------------------------------- /Utils/quadrotor_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 | #uint8[4] motor_rpm 12 | uint8 seq 13 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/PPROutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 quad_time 3 | float64 des_thrust 4 | float64 des_roll 5 | float64 des_pitch 6 | float64 des_yaw 7 | float64 est_roll 8 | float64 est_pitch 9 | float64 est_yaw 10 | float64 est_angvel_x 11 | float64 est_angvel_y 12 | float64 est_angvel_z 13 | float64 est_acc_x 14 | float64 est_acc_y 15 | float64 est_acc_z 16 | uint16[4] pwm 17 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/PolynomialMatrix.msg: -------------------------------------------------------------------------------- 1 | # the order of trajectory. 2 | uint32 num_order 3 | uint32 num_dim 4 | 5 | # the polynomial coecfficients of the trajectory. 6 | 7 | float64[] data 8 | float64 duration 9 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/PolynomialTraj.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the order of trajectory. 15 | quadrotor_msgs/PolynomialMatrix[] trajectory 16 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the order of trajectory. 15 | uint32 num_order 16 | uint32 num_segment 17 | 18 | # the polynomial coecfficients of the trajectory. 19 | float64 start_yaw 20 | float64 final_yaw 21 | float64[] coef_x 22 | float64[] coef_y 23 | float64[] coef_z 24 | float64[] time 25 | float64 mag_coeff 26 | uint32[] order 27 | 28 | string debug_info 29 | -------------------------------------------------------------------------------- /Utils/quadrotor_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 | uint32 trajectory_id 12 | 13 | uint8 TRAJECTORY_STATUS_EMPTY = 0 14 | uint8 TRAJECTORY_STATUS_READY = 1 15 | uint8 TRAJECTORY_STATUS_COMPLETED = 3 16 | uint8 TRAJECTROY_STATUS_ABORT = 4 17 | uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5 18 | uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6 19 | uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7 20 | 21 | # Its ID number will start from 1, allowing you comparing it with 0. 22 | uint8 trajectory_flag 23 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Px4ctrlDebug.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float64 des_v_x 4 | float64 des_v_y 5 | float64 des_v_z 6 | 7 | float64 fb_a_x 8 | float64 fb_a_y 9 | float64 fb_a_z 10 | 11 | float64 des_a_x 12 | float64 des_a_y 13 | float64 des_a_z 14 | 15 | float64 des_q_x 16 | float64 des_q_y 17 | float64 des_q_z 18 | float64 des_q_w 19 | 20 | float64 des_thr 21 | float64 hover_percentage 22 | float64 thr_scale_compensate 23 | float64 voltage 24 | 25 | float64 err_axisang_x 26 | float64 err_axisang_y 27 | float64 err_axisang_z 28 | float64 err_axisang_ang 29 | 30 | float64 fb_rate_x 31 | float64 fb_rate_y 32 | float64 fb_rate_z 33 | 34 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/SO3Command.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 force 3 | geometry_msgs/Quaternion orientation 4 | float64[3] kR 5 | float64[3] kOm 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Serial.msg: -------------------------------------------------------------------------------- 1 | # Note: These constants need to be kept in sync with the types 2 | # defined in include/quadrotor_msgs/comm_types.h 3 | uint8 SO3_CMD = 115 # 's' in base 10 4 | uint8 TRPY_CMD = 112 # 'p' in base 10 5 | uint8 STATUS_DATA = 99 # 'c' in base 10 6 | uint8 OUTPUT_DATA = 100 # 'd' in base 10 7 | uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 8 | uint8 PPR_GAINS = 103 # 'g' 9 | 10 | Header header 11 | uint8 channel 12 | uint8 type # One of the types listed above 13 | uint8[] data 14 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/StatusData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | uint8 seq 5 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/TRPYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 thrust 3 | float32 roll 4 | float32 pitch 5 | float32 yaw 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/TakeoffLand.msg: -------------------------------------------------------------------------------- 1 | uint8 TAKEOFF = 1 2 | uint8 LAND = 2 3 | uint8 takeoff_land_cmd -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/msg/Trajectory.msg: -------------------------------------------------------------------------------- 1 | # Trajectory type enums 2 | 3 | # Undefined trajectory type 4 | uint8 UNDEFINED=0 5 | 6 | # General trajectory type that considers orientation from the pose and 7 | # neglects heading values 8 | uint8 GENERAL=1 9 | 10 | # Trajectory types that compute orientation from acceleration and heading 11 | # values and consider derivatives up to what is indicated by the name 12 | uint8 ACCELERATION=2 13 | uint8 JERK=3 14 | uint8 SNAP=4 15 | 16 | Header header 17 | 18 | # Trajectory type as defined above 19 | uint8 type 20 | 21 | quadrotor_msgs/TrajectoryPoint[] points 22 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | quadrotor_msgs 3 | 0.0.0 4 | quadrotor_msgs 5 | Kartik Mohta 6 | http://ros.org/wiki/quadrotor_msgs 7 | BSD 8 | catkin 9 | geometry_msgs 10 | nav_msgs 11 | message_generation 12 | geometry_msgs 13 | nav_msgs 14 | message_runtime 15 | 16 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/src/decode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/decode_msgs.h" 2 | #include 3 | #include 4 | 5 | namespace quadrotor_msgs 6 | { 7 | 8 | bool decodeOutputData(const std::vector &data, 9 | quadrotor_msgs::OutputData &output) 10 | { 11 | struct OUTPUT_DATA output_data; 12 | if(data.size() != sizeof(output_data)) 13 | return false; 14 | 15 | memcpy(&output_data, &data[0], sizeof(output_data)); 16 | output.loop_rate = output_data.loop_rate; 17 | output.voltage = output_data.voltage/1e3; 18 | 19 | const double roll = output_data.roll/1e2 * M_PI/180; 20 | const double pitch = output_data.pitch/1e2 * M_PI/180; 21 | const double yaw = output_data.yaw/1e2 * M_PI/180; 22 | // Asctec (2012 firmware) uses Z-Y-X convention 23 | Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 24 | Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 25 | Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); 26 | output.orientation.w = q.w(); 27 | output.orientation.x = q.x(); 28 | output.orientation.y = q.y(); 29 | output.orientation.z = q.z(); 30 | 31 | output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180; 32 | output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180; 33 | output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180; 34 | 35 | output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81; 36 | output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81; 37 | output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81; 38 | 39 | output.pressure_dheight = output_data.dheight/1e3; 40 | output.pressure_height = output_data.height/1e3; 41 | 42 | output.magnetic_field.x = output_data.mag[0]/2500.0; 43 | output.magnetic_field.y = output_data.mag[1]/2500.0; 44 | output.magnetic_field.z = output_data.mag[2]/2500.0; 45 | 46 | for(int i = 0; i < 8; i++) 47 | { 48 | output.radio_channel[i] = output_data.radio[i]; 49 | } 50 | //for(int i = 0; i < 4; i++) 51 | // output.motor_rpm[i] = output_data.rpm[i]; 52 | 53 | output.seq = output_data.seq; 54 | 55 | return true; 56 | } 57 | 58 | bool decodeStatusData(const std::vector &data, 59 | quadrotor_msgs::StatusData &status) 60 | { 61 | struct STATUS_DATA status_data; 62 | if(data.size() != sizeof(status_data)) 63 | return false; 64 | memcpy(&status_data, &data[0], sizeof(status_data)); 65 | 66 | status.loop_rate = status_data.loop_rate; 67 | status.voltage = status_data.voltage/1e3; 68 | status.seq = status_data.seq; 69 | 70 | return true; 71 | } 72 | 73 | bool decodePPROutputData(const std::vector &data, 74 | quadrotor_msgs::PPROutputData &output) 75 | { 76 | struct PPR_OUTPUT_DATA output_data; 77 | if(data.size() != sizeof(output_data)) 78 | return false; 79 | memcpy(&output_data, &data[0], sizeof(output_data)); 80 | 81 | output.quad_time = output_data.time; 82 | output.des_thrust = output_data.des_thrust*1e-4; 83 | output.des_roll = output_data.des_roll*1e-4; 84 | output.des_pitch = output_data.des_pitch*1e-4; 85 | output.des_yaw = output_data.des_yaw*1e-4; 86 | output.est_roll = output_data.est_roll*1e-4; 87 | output.est_pitch = output_data.est_pitch*1e-4; 88 | output.est_yaw = output_data.est_yaw*1e-4; 89 | output.est_angvel_x = output_data.est_angvel_x*1e-3; 90 | output.est_angvel_y = output_data.est_angvel_y*1e-3; 91 | output.est_angvel_z = output_data.est_angvel_z*1e-3; 92 | output.est_acc_x = output_data.est_acc_x*1e-4; 93 | output.est_acc_y = output_data.est_acc_y*1e-4; 94 | output.est_acc_z = output_data.est_acc_z*1e-4; 95 | output.pwm[0] = output_data.pwm1; 96 | output.pwm[1] = output_data.pwm2; 97 | output.pwm[2] = output_data.pwm3; 98 | output.pwm[3] = output_data.pwm4; 99 | 100 | return true; 101 | } 102 | 103 | } 104 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/src/encode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/encode_msgs.h" 2 | #include 3 | 4 | namespace quadrotor_msgs 5 | { 6 | 7 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 8 | std::vector &output) 9 | { 10 | struct SO3_CMD_INPUT so3_cmd_input; 11 | 12 | so3_cmd_input.force[0] = so3_command.force.x*500; 13 | so3_cmd_input.force[1] = so3_command.force.y*500; 14 | so3_cmd_input.force[2] = so3_command.force.z*500; 15 | 16 | so3_cmd_input.des_qx = so3_command.orientation.x*125; 17 | so3_cmd_input.des_qy = so3_command.orientation.y*125; 18 | so3_cmd_input.des_qz = so3_command.orientation.z*125; 19 | so3_cmd_input.des_qw = so3_command.orientation.w*125; 20 | 21 | so3_cmd_input.kR[0] = so3_command.kR[0]*50; 22 | so3_cmd_input.kR[1] = so3_command.kR[1]*50; 23 | so3_cmd_input.kR[2] = so3_command.kR[2]*50; 24 | 25 | so3_cmd_input.kOm[0] = so3_command.kOm[0]*100; 26 | so3_cmd_input.kOm[1] = so3_command.kOm[1]*100; 27 | so3_cmd_input.kOm[2] = so3_command.kOm[2]*100; 28 | 29 | so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4; 30 | 31 | so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11; 32 | so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500; 33 | so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500; 34 | 35 | so3_cmd_input.enable_motors = so3_command.aux.enable_motors; 36 | so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; 37 | 38 | so3_cmd_input.seq = so3_command.header.seq % 255; 39 | 40 | output.resize(sizeof(so3_cmd_input)); 41 | memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); 42 | } 43 | 44 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 45 | std::vector &output) 46 | { 47 | struct TRPY_CMD trpy_cmd_input; 48 | trpy_cmd_input.thrust = trpy_command.thrust*1e4; 49 | trpy_cmd_input.roll = trpy_command.roll*1e4; 50 | trpy_cmd_input.pitch = trpy_command.pitch*1e4; 51 | trpy_cmd_input.yaw = trpy_command.yaw*1e4; 52 | trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4; 53 | trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; 54 | trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; 55 | 56 | output.resize(sizeof(trpy_cmd_input)); 57 | memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); 58 | } 59 | 60 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 61 | std::vector &output) 62 | { 63 | struct PPR_GAINS ppr_gains; 64 | ppr_gains.Kp = gains.Kp; 65 | ppr_gains.Kd = gains.Kd; 66 | ppr_gains.Kp_yaw = gains.Kp_yaw; 67 | ppr_gains.Kd_yaw = gains.Kd_yaw; 68 | 69 | output.resize(sizeof(ppr_gains)); 70 | memcpy(&output[0], &ppr_gains, sizeof(ppr_gains)); 71 | } 72 | 73 | } 74 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py: -------------------------------------------------------------------------------- 1 | """autogenerated by genpy from quadrotor_msgs/Gains.msg. Do not edit.""" 2 | import sys 3 | python3 = True if sys.hexversion > 0x03000000 else False 4 | import genpy 5 | import struct 6 | 7 | 8 | class Gains(genpy.Message): 9 | _md5sum = "b82763b9f24e5595e2a816aa779c9461" 10 | _type = "quadrotor_msgs/Gains" 11 | _has_header = False #flag to mark the presence of a Header object 12 | _full_text = """float64 Kp 13 | float64 Kd 14 | float64 Kp_yaw 15 | float64 Kd_yaw 16 | 17 | """ 18 | __slots__ = ['Kp','Kd','Kp_yaw','Kd_yaw'] 19 | _slot_types = ['float64','float64','float64','float64'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | Kp,Kd,Kp_yaw,Kd_yaw 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(Gains, self).__init__(*args, **kwds) 37 | #message fields cannot be None, assign default values for those that are 38 | if self.Kp is None: 39 | self.Kp = 0. 40 | if self.Kd is None: 41 | self.Kd = 0. 42 | if self.Kp_yaw is None: 43 | self.Kp_yaw = 0. 44 | if self.Kd_yaw is None: 45 | self.Kd_yaw = 0. 46 | else: 47 | self.Kp = 0. 48 | self.Kd = 0. 49 | self.Kp_yaw = 0. 50 | self.Kd_yaw = 0. 51 | 52 | def _get_types(self): 53 | """ 54 | internal API method 55 | """ 56 | return self._slot_types 57 | 58 | def serialize(self, buff): 59 | """ 60 | serialize message into buffer 61 | :param buff: buffer, ``StringIO`` 62 | """ 63 | try: 64 | _x = self 65 | buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) 66 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 67 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 68 | 69 | def deserialize(self, str): 70 | """ 71 | unpack serialized message in str into this message instance 72 | :param str: byte array of serialized message, ``str`` 73 | """ 74 | try: 75 | end = 0 76 | _x = self 77 | start = end 78 | end += 32 79 | (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) 80 | return self 81 | except struct.error as e: 82 | raise genpy.DeserializationError(e) #most likely buffer underfill 83 | 84 | 85 | def serialize_numpy(self, buff, numpy): 86 | """ 87 | serialize message with numpy array types into buffer 88 | :param buff: buffer, ``StringIO`` 89 | :param numpy: numpy python module 90 | """ 91 | try: 92 | _x = self 93 | buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) 94 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x)))) 95 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x)))) 96 | 97 | def deserialize_numpy(self, str, numpy): 98 | """ 99 | unpack serialized message in str into this message instance using numpy for array types 100 | :param str: byte array of serialized message, ``str`` 101 | :param numpy: numpy python module 102 | """ 103 | try: 104 | end = 0 105 | _x = self 106 | start = end 107 | end += 32 108 | (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) 109 | return self 110 | except struct.error as e: 111 | raise genpy.DeserializationError(e) #most likely buffer underfill 112 | 113 | _struct_I = genpy.struct_I 114 | _struct_4d = struct.Struct("<4d") 115 | -------------------------------------------------------------------------------- /Utils/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._Gains import * 2 | from ._SO3Command import * 3 | from ._TRPYCommand import * 4 | from ._PositionCommand import * 5 | from ._PPROutputData import * 6 | from ._OutputData import * 7 | from ._Corrections import * 8 | from ._Serial import * 9 | from ._AuxCommand import * 10 | from ._StatusData import * 11 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_plugins) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rviz 9 | ) 10 | 11 | catkin_package( 12 | # INCLUDE_DIRS include 13 | # LIBRARIES irobot_msgs 14 | DEPENDS system_lib 15 | message_runtime 16 | ) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | if(rviz_QT_VERSION VERSION_LESS "5") 23 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 24 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 25 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 26 | include(${QT_USE_FILE}) 27 | qt4_wrap_cpp(MOC_FILES 28 | src/goal_tool.h 29 | ) 30 | 31 | else() 32 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 33 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 34 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 35 | set(QT_LIBRARIES Qt5::Widgets) 36 | qt5_wrap_cpp(MOC_FILES 37 | src/goal_tool.h 38 | ) 39 | endif() 40 | 41 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 42 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 43 | add_definitions(-DQT_NO_KEYWORDS) 44 | 45 | ## Here we specify which header files need to be run through "moc", 46 | ## Qt's meta-object compiler. 47 | 48 | ## Here we specify the list of source files, including the output of 49 | ## the previous command which is stored in ``${MOC_FILES}``. 50 | set(SOURCE_FILES 51 | src/pose_tool.cpp 52 | src/goal_tool.cpp 53 | ${MOC_FILES} 54 | ) 55 | 56 | ## Specify additional locations of header files 57 | ## Your package locations should be listed before other locations 58 | include_directories(include) 59 | include_directories( 60 | ${catkin_INCLUDE_DIRS} 61 | ) 62 | 63 | add_library(${PROJECT_NAME} ${SOURCE_FILES}) 64 | 65 | add_dependencies(${PROJECT_NAME} multi_map_server_generate_messages_cpp) 66 | 67 | target_link_libraries(${PROJECT_NAME} 68 | ${catkin_LIBRARIES} 69 | ${QT_LIBRARIES} 70 | ) 71 | install(TARGETS 72 | ${PROJECT_NAME} 73 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 74 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 76 | ) 77 | 78 | install(FILES 79 | plugin_description.xml 80 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 81 | 82 | install(DIRECTORY media/ 83 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media) 84 | 85 | install(DIRECTORY icons/ 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 87 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_plugins 3 | 4 | 5 | Additional plugins for rviz 6 | 7 | 8 | 0.1.0 9 | Shaojie Shen 10 | william.wu 11 | LGPLv3 12 | http://ros.org/wiki/rviz_plugins 13 | 14 | catkin 15 | 16 | rviz 17 | roscpp 18 | multi_map_server 19 | qtbase5-dev 20 | message_runtime 21 | 22 | rviz 23 | multi_map_server 24 | message_runtime 25 | roscpp 26 | libqt5-core 27 | libqt5-gui 28 | libqt5-widgets 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Tool for setting 3D goal 7 | 8 | 9 | 10 | 13 | 14 | Display of -inf +inf probabilistic occupancy grid map 15 | 16 | 17 | 18 | 21 | 22 | game usage 23 | 24 | 25 | 26 | 29 | 30 | Display of aerial map 31 | 32 | 33 | 34 | 37 | 38 | Display of multiple -inf +inf probabilistic occupancy grid map 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/src/goal_tool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include "rviz/display_context.h" 35 | #include "rviz/properties/string_property.h" 36 | 37 | #include "goal_tool.h" 38 | 39 | namespace rviz 40 | { 41 | 42 | Goal3DTool::Goal3DTool() 43 | { 44 | shortcut_key_ = 'g'; 45 | 46 | topic_property_ = new StringProperty( "Topic", "goal", 47 | "The topic on which to publish navigation goals.", 48 | getPropertyContainer(), SLOT( updateTopic() ), this ); 49 | } 50 | 51 | void Goal3DTool::onInitialize() 52 | { 53 | Pose3DTool::onInitialize(); 54 | setName( "3D Nav Goal" ); 55 | updateTopic(); 56 | } 57 | 58 | void Goal3DTool::updateTopic() 59 | { 60 | pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); 61 | } 62 | 63 | void Goal3DTool::onPoseSet(double x, double y, double z, double theta) 64 | { 65 | ROS_WARN("3D Goal Set"); 66 | std::string fixed_frame = context_->getFixedFrame().toStdString(); 67 | tf::Quaternion quat; 68 | quat.setRPY(0.0, 0.0, theta); 69 | tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, z)), ros::Time::now(), fixed_frame); 70 | geometry_msgs::PoseStamped goal; 71 | tf::poseStampedTFToMsg(p, goal); 72 | ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), 73 | goal.pose.position.x, goal.pose.position.y, goal.pose.position.z, 74 | goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta); 75 | pub_.publish(goal); 76 | } 77 | 78 | } // end namespace rviz 79 | 80 | #include 81 | PLUGINLIB_EXPORT_CLASS( rviz::Goal3DTool, rviz::Tool ) 82 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/src/goal_tool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef RVIZ_GOAL_TOOL_H 31 | #define RVIZ_GOAL_TOOL_H 32 | 33 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 34 | # include 35 | 36 | # include 37 | 38 | # include "pose_tool.h" 39 | #endif 40 | 41 | namespace rviz 42 | { 43 | class Arrow; 44 | class DisplayContext; 45 | class StringProperty; 46 | 47 | class Goal3DTool: public Pose3DTool 48 | { 49 | Q_OBJECT 50 | public: 51 | Goal3DTool(); 52 | virtual ~Goal3DTool() {} 53 | virtual void onInitialize(); 54 | 55 | protected: 56 | virtual void onPoseSet(double x, double y, double z, double theta); 57 | 58 | private Q_SLOTS: 59 | void updateTopic(); 60 | 61 | private: 62 | ros::NodeHandle nh_; 63 | ros::Publisher pub_; 64 | 65 | StringProperty* topic_property_; 66 | }; 67 | 68 | } 69 | 70 | #endif 71 | 72 | 73 | -------------------------------------------------------------------------------- /Utils/rviz_plugins/src/pose_tool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef RVIZ_POSE_TOOL_H 31 | #define RVIZ_POSE_TOOL_H 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "rviz/tool.h" 40 | 41 | namespace rviz 42 | { 43 | class Arrow; 44 | class DisplayContext; 45 | 46 | class Pose3DTool : public Tool 47 | { 48 | public: 49 | Pose3DTool(); 50 | virtual ~Pose3DTool(); 51 | 52 | virtual void onInitialize(); 53 | 54 | virtual void activate(); 55 | virtual void deactivate(); 56 | 57 | virtual int processMouseEvent(ViewportMouseEvent& event); 58 | 59 | protected: 60 | virtual void onPoseSet(double x, double y, double z, double theta) = 0; 61 | 62 | Arrow* arrow_; 63 | std::vector arrow_array; 64 | 65 | enum State 66 | { 67 | Position, 68 | Orientation, 69 | Height 70 | }; 71 | State state_; 72 | 73 | Ogre::Vector3 pos_; 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /Utils/uav_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uav_utils) 3 | 4 | ## Turn on verbose output 5 | set(CMAKE_VERBOSE_MAKEFILE "false") 6 | 7 | ## Enable C++11 8 | include(CheckCXXCompilerFlag) 9 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 10 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 11 | if(COMPILER_SUPPORTS_CXX11) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 13 | elseif(COMPILER_SUPPORTS_CXX0X) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 15 | else() 16 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 17 | endif() 18 | 19 | ## Add some compile flags 20 | set(ADDITIONAL_CXX_FLAG "-Wall -O3 -g") 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") 22 | 23 | ## Find catkin macros and libraries 24 | find_package(catkin REQUIRED COMPONENTS 25 | roscpp 26 | rospy 27 | # cmake_utils 28 | ) 29 | 30 | 31 | find_package(Eigen3 REQUIRED) # try to find manually installed eigen (Usually in /usr/local with provided FindEigen3.cmake) 32 | 33 | ################################### 34 | ## catkin specific configuration ## 35 | ################################### 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | # LIBRARIES uav_utils 39 | CATKIN_DEPENDS roscpp rospy 40 | # DEPENDS system_lib 41 | ) 42 | 43 | ########### 44 | ## Build ## 45 | ########### 46 | 47 | 48 | include_directories( 49 | include 50 | ${EIGEN_INCLUDE_DIRS} 51 | ${catkin_INCLUDE_DIRS} 52 | ) 53 | 54 | ############# 55 | ## Install ## 56 | ############# 57 | 58 | # all install targets should use catkin DESTINATION variables 59 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 60 | 61 | ## Mark executable scripts (Python etc.) for installation 62 | ## in contrast to setup.py, you can choose the destination 63 | # install(PROGRAMS 64 | # scripts/my_python_script 65 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | # ) 67 | 68 | ## Mark executables and/or libraries for installation 69 | # install(TARGETS geometry_utils geometry_utils_node 70 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 73 | # ) 74 | 75 | ## Mark cpp header files for installation 76 | install(DIRECTORY include/${PROJECT_NAME}/ 77 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | FILES_MATCHING PATTERN "*.h" 79 | PATTERN ".svn" EXCLUDE 80 | ) 81 | 82 | ## Mark other files for installation (e.g. launch and bag files, etc.) 83 | # install(FILES 84 | # # myfile1 85 | # # myfile2 86 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | # ) 88 | 89 | ############# 90 | ## Testing ## 91 | ############# 92 | 93 | ## Add gtest based cpp test target and link libraries 94 | catkin_add_gtest(${PROJECT_NAME}-test src/${PROJECT_NAME}_test.cpp) 95 | if(TARGET ${PROJECT_NAME}-test) 96 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 97 | endif() 98 | 99 | ## Add folders to be run by python nosetests 100 | # catkin_add_nosetests(test) 101 | -------------------------------------------------------------------------------- /Utils/uav_utils/include/uav_utils/converters.h: -------------------------------------------------------------------------------- 1 | #ifndef __UAVUTILS_CONVERTERS_H 2 | #define __UAVUTILS_CONVERTERS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uav_utils { 14 | 15 | inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, 16 | Eigen::Vector3d& v, Eigen::Quaterniond& q) 17 | { 18 | p(0) = msg->pose.pose.position.x; 19 | p(1) = msg->pose.pose.position.y; 20 | p(2) = msg->pose.pose.position.z; 21 | 22 | v(0) = msg->twist.twist.linear.x; 23 | v(1) = msg->twist.twist.linear.y; 24 | v(2) = msg->twist.twist.linear.z; 25 | 26 | q.w() = msg->pose.pose.orientation.w; 27 | q.x() = msg->pose.pose.orientation.x; 28 | q.y() = msg->pose.pose.orientation.y; 29 | q.z() = msg->pose.pose.orientation.z; 30 | } 31 | 32 | inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, 33 | Eigen::Vector3d& v, Eigen::Quaterniond& q, Eigen::Vector3d& w) 34 | { 35 | extract_odometry(msg, p, v, q); 36 | 37 | w(0) = msg->twist.twist.angular.x; 38 | w(1) = msg->twist.twist.angular.y; 39 | w(2) = msg->twist.twist.angular.z; 40 | } 41 | 42 | 43 | template 44 | Eigen::Matrix from_vector3_msg(const geometry_msgs::Vector3& msg) { 45 | return Eigen::Matrix(msg.x, msg.y, msg.z); 46 | } 47 | 48 | template 49 | geometry_msgs::Vector3 to_vector3_msg(const Eigen::DenseBase& v) { 50 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 51 | EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); 52 | EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); 53 | 54 | geometry_msgs::Vector3 msg; 55 | msg.x = v.x(); 56 | msg.y = v.y(); 57 | msg.z = v.z(); 58 | return msg; 59 | } 60 | 61 | template 62 | Eigen::Matrix from_point_msg(const geometry_msgs::Point& msg) { 63 | return Eigen::Matrix(msg.x, msg.y, msg.z); 64 | } 65 | 66 | template 67 | geometry_msgs::Point to_point_msg(const Eigen::DenseBase& v) { 68 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 69 | EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); 70 | EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); 71 | 72 | geometry_msgs::Point msg; 73 | msg.x = v.x(); 74 | msg.y = v.y(); 75 | msg.z = v.z(); 76 | return msg; 77 | } 78 | 79 | template 80 | Eigen::Quaternion from_quaternion_msg(const geometry_msgs::Quaternion& msg) { 81 | return Eigen::Quaternion(msg.w, msg.x, msg.y, msg.z); 82 | } 83 | 84 | template 85 | geometry_msgs::Quaternion to_quaternion_msg(const Eigen::Quaternion& q) { 86 | geometry_msgs::Quaternion msg; 87 | msg.x = q.x(); 88 | msg.y = q.y(); 89 | msg.z = q.z(); 90 | msg.w = q.w(); 91 | return msg; 92 | } 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /Utils/uav_utils/include/uav_utils/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef __UAV_UTILS_H 2 | #define __UAV_UTILS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace uav_utils 10 | { 11 | 12 | /* judge if value belongs to [low,high] */ 13 | template 14 | bool 15 | in_range(T value, const T2& low, const T2& high) 16 | { 17 | ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); 18 | return (low <= value) && (value <= high); 19 | } 20 | 21 | /* judge if value belongs to [-limit, limit] */ 22 | template 23 | bool 24 | in_range(T value, const T2& limit) 25 | { 26 | ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); 27 | return in_range(value, -limit, limit); 28 | } 29 | 30 | template 31 | void 32 | limit_range(T& value, const T2& low, const T2& high) 33 | { 34 | ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); 35 | if (value < low) 36 | { 37 | value = low; 38 | } 39 | 40 | if (value > high) 41 | { 42 | value = high; 43 | } 44 | 45 | return; 46 | } 47 | 48 | template 49 | void 50 | limit_range(T& value, const T2& limit) 51 | { 52 | ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); 53 | limit_range(value, -limit, limit); 54 | } 55 | 56 | typedef std::stringstream DebugSS_t; 57 | } // end of namespace uav_utils 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /Utils/uav_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uav_utils 4 | 0.0.0 5 | The uav_utils package 6 | 7 | 8 | 9 | 10 | LIU Tianbo 11 | 12 | 13 | 14 | 15 | 16 | LGPLv3 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 | catkin 43 | roscpp 44 | rospy 45 | cmake_modules 46 | cmake_utils 47 | roscpp 48 | rospy 49 | cmake_modules 50 | cmake_utils 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /Utils/uav_utils/scripts/odom_to_euler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import numpy as np 5 | import tf 6 | from tf import transformations as tfs 7 | from nav_msgs.msg import Odometry 8 | from sensor_msgs.msg import Imu 9 | from geometry_msgs.msg import Vector3Stamped 10 | from sensor_msgs.msg import Joy 11 | 12 | pub = None 13 | pub1 = None 14 | 15 | def callback(odom_msg): 16 | q = np.array([odom_msg.pose.pose.orientation.x, 17 | odom_msg.pose.pose.orientation.y, 18 | odom_msg.pose.pose.orientation.z, 19 | odom_msg.pose.pose.orientation.w]) 20 | 21 | e = tfs.euler_from_quaternion(q, 'rzyx') 22 | 23 | euler_msg = Vector3Stamped() 24 | euler_msg.header = odom_msg.header 25 | euler_msg.vector.z = e[0]*180.0/3.14159 26 | euler_msg.vector.y = e[1]*180.0/3.14159 27 | euler_msg.vector.x = e[2]*180.0/3.14159 28 | 29 | pub.publish(euler_msg) 30 | 31 | def imu_callback(imu_msg): 32 | q = np.array([imu_msg.orientation.x, 33 | imu_msg.orientation.y, 34 | imu_msg.orientation.z, 35 | imu_msg.orientation.w]) 36 | 37 | e = tfs.euler_from_quaternion(q, 'rzyx') 38 | 39 | euler_msg = Vector3Stamped() 40 | euler_msg.header = imu_msg.header 41 | euler_msg.vector.z = e[0]*180.0/3.14159 42 | euler_msg.vector.y = e[1]*180.0/3.14159 43 | euler_msg.vector.x = e[2]*180.0/3.14159 44 | 45 | pub1.publish(euler_msg) 46 | 47 | def joy_callback(joy_msg): 48 | out_msg = Vector3Stamped() 49 | out_msg.header = joy_msg.header 50 | out_msg.vector.z = -joy_msg.axes[3] 51 | out_msg.vector.y = joy_msg.axes[1] 52 | out_msg.vector.x = joy_msg.axes[0] 53 | 54 | pub2.publish(out_msg) 55 | 56 | 57 | if __name__ == "__main__": 58 | rospy.init_node("odom_to_euler") 59 | 60 | pub = rospy.Publisher("~euler", Vector3Stamped, queue_size=10) 61 | sub = rospy.Subscriber("~odom", Odometry, callback) 62 | 63 | pub1 = rospy.Publisher("~imueuler", Vector3Stamped, queue_size=10) 64 | sub1 = rospy.Subscriber("~imu", Imu, imu_callback) 65 | 66 | pub2 = rospy.Publisher("~ctrlout", Vector3Stamped, queue_size=10) 67 | sub2 = rospy.Subscriber("~ctrlin", Joy, joy_callback) 68 | 69 | rospy.spin() 70 | -------------------------------------------------------------------------------- /Utils/uav_utils/scripts/send_odom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import numpy as np 5 | import tf 6 | from tf import transformations as tfs 7 | from nav_msgs.msg import Odometry 8 | 9 | if __name__ == "__main__": 10 | rospy.init_node("odom_sender") 11 | 12 | msg = Odometry() 13 | 14 | msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) 15 | msg.header.frame_id = "world" 16 | 17 | q = tfs.quaternion_from_euler(0,0,0,"rzyx") 18 | 19 | msg.pose.pose.position.x = 0 20 | msg.pose.pose.position.y = 0 21 | msg.pose.pose.position.z = 0 22 | msg.twist.twist.linear.x = 0 23 | msg.twist.twist.linear.y = 0 24 | msg.twist.twist.linear.z = 0 25 | msg.pose.pose.orientation.x = q[0] 26 | msg.pose.pose.orientation.y = q[1] 27 | msg.pose.pose.orientation.z = q[2] 28 | msg.pose.pose.orientation.w = q[3] 29 | 30 | print(msg) 31 | 32 | pub = rospy.Publisher("odom", Odometry, queue_size=10) 33 | 34 | counter = 0 35 | r = rospy.Rate(1) 36 | 37 | while not rospy.is_shutdown(): 38 | counter += 1 39 | msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) 40 | pub.publish(msg) 41 | rospy.loginfo("Send %3d msg(s)."%counter) 42 | r.sleep() 43 | -------------------------------------------------------------------------------- /Utils/uav_utils/scripts/topic_statistics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse as ap 4 | import argcomplete 5 | 6 | def main(**args): 7 | pass 8 | 9 | if __name__ == '__main__': 10 | parser = ap.ArgumentParser() 11 | parser.add_argument('positional', choices=['spam', 'eggs']) 12 | parser.add_argument('--optional', choices=['foo1', 'foo2', 'bar']) 13 | argcomplete.autocomplete(parser) 14 | args = parser.parse_args() 15 | main(**vars(args)) -------------------------------------------------------------------------------- /controller/payload_mpc_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(payload_mpc_controller) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 6 | 7 | # ARM NEON flags 8 | if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l") 9 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations") 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations") 11 | message("enabling ARM neon optimizations") 12 | endif() 13 | 14 | # flags for speed (should already be enabled by default) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 16 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g") 17 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -fopenmp -O3 -march=native") 18 | 19 | find_package(Eigen3 REQUIRED) 20 | find_package(catkin REQUIRED COMPONENTS 21 | roscpp 22 | std_msgs 23 | geometry_msgs 24 | quadrotor_msgs 25 | uav_utils 26 | 27 | ) 28 | 29 | catkin_package() 30 | 31 | #include dir 32 | # include_directories( 33 | # include 34 | # include/payload_mpc_controller 35 | # ) 36 | 37 | # add_library(mpc_solver 38 | # externals/qpoases/SRC/Bounds.cpp 39 | # externals/qpoases/SRC/Constraints.cpp 40 | # externals/qpoases/SRC/CyclingManager.cpp 41 | # externals/qpoases/SRC/Indexlist.cpp 42 | # externals/qpoases/SRC/MessageHandling.cpp 43 | # externals/qpoases/SRC/QProblem.cpp 44 | # externals/qpoases/SRC/QProblemB.cpp 45 | # externals/qpoases/SRC/SubjectTo.cpp 46 | # externals/qpoases/SRC/Utils.cpp 47 | # externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp 48 | # model/quadrotor_payload_mpc/acado_qpoases_interface.cpp 49 | # model/quadrotor_payload_mpc/acado_integrator.c 50 | # model/quadrotor_payload_mpc/acado_solver.c 51 | # model/quadrotor_payload_mpc/acado_auxiliary_functions.c) 52 | 53 | # target_include_directories(mpc_solver PUBLIC 54 | # model/quadrotor_payload_mpc/ 55 | # externals/qpoases 56 | # externals/qpoases/INCLUDE 57 | # externals/qpoases/SRC 58 | # ) 59 | 60 | add_executable(mpc_controller_node 61 | src/mpc_controller_node.cpp 62 | src/mpc_controller.cpp 63 | src/mpc_input.cpp 64 | src/mpc_fsm.cpp 65 | src/mpc_wrapper.cpp 66 | # MPC solver 67 | externals/qpoases/SRC/Bounds.cpp 68 | externals/qpoases/SRC/Constraints.cpp 69 | externals/qpoases/SRC/CyclingManager.cpp 70 | externals/qpoases/SRC/Indexlist.cpp 71 | externals/qpoases/SRC/MessageHandling.cpp 72 | externals/qpoases/SRC/QProblem.cpp 73 | externals/qpoases/SRC/QProblemB.cpp 74 | externals/qpoases/SRC/SubjectTo.cpp 75 | externals/qpoases/SRC/Utils.cpp 76 | externals/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp 77 | model/quadrotor_payload_mpc/acado_qpoases_interface.cpp 78 | model/quadrotor_payload_mpc/acado_integrator.c 79 | model/quadrotor_payload_mpc/acado_solver.c 80 | model/quadrotor_payload_mpc/acado_auxiliary_functions.c 81 | ) 82 | target_include_directories(mpc_controller_node PUBLIC 83 | include/payload_mpc_controller 84 | include 85 | ${EIGEN3_INCLUDE_DIR} 86 | SYSTEM 87 | ${catkin_INCLUDE_DIRS} 88 | # MPC solver 89 | model/quadrotor_payload_mpc/ 90 | externals/qpoases 91 | externals/qpoases/INCLUDE 92 | externals/qpoases/SRC 93 | ) 94 | target_link_libraries(mpc_controller_node 95 | # mpc_solver 96 | ${catkin_LIBRARIES} 97 | ) 98 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/config/model.yaml: -------------------------------------------------------------------------------- 1 | # Model parameters 2 | mass_q: 1.150 3 | l_length: 0.66844507 4 | mass_l: 0.285 5 | 6 | # !!! if the below model parameters has been changed, 7 | # !!! please re-run the quadrotor_payload_mpc_with_ext_force_codegen to re-generate the MPC model code. !!! 8 | gravity: 9.795 9 | step_N: 20 10 | step_T: 0.041 11 | force_estimator: # * 12 | USE_CONSTANT_MOMENT: 1 # Same as the code 13 | #0 mean close 14 | #1 mean use fl as constant moment 15 | #2 mean use fq as constant moment -------------------------------------------------------------------------------- /controller/payload_mpc_controller/config/mpc.yaml: -------------------------------------------------------------------------------- 1 | # using receved time (only for one traj ) 2 | use_simulation: true 3 | use_fix_yaw: false # using fix yaw. 4 | 5 | # Cost on states 6 | Q_pos_xy: 250.0 # Cost for horizontal positon error //default: 200 0.5 7 | Q_pos_z: 250.0 # Cost for horizontal positon error //default: 200 0.5 8 | Q_attitude_rp: 120.0 # Cost for attitude error //default: 50 0.1 9 | Q_attitude_yaw: 120.0 # Cost for attitude error //default: 50 0.1 10 | Q_velocity: 25.0 # Cost for velocity error //default: 10 0.05 11 | 12 | Q_payload_xy: 20.0 13 | Q_payload_z: 20.0 14 | Q_payload_velocity: 3.0 15 | Q_cable: 0.0 16 | Q_dcable: 0.0 17 | 18 | # Cost on Inputs 19 | R_thrust: 0.75 # Cost on thrust input //default: 1 20 | R_pitchroll: 0.75 # Cost on pitch and roll rate 21 | R_yaw: 0.75 # Cost on yaw ratte 22 | 23 | # Exponential scaling: W_i = W * exp(-i/N * cost_scaling). 24 | # cost_scaling = 0 means no scaling W_i = W. 25 | state_cost_exponential: 0.5 # Scaling for state costs 26 | input_cost_exponential: 0.5 # scaling for input costs 27 | 28 | # Limits for inputs 29 | max_bodyrate_xy: 3.0 # ~ pi [rad/s] 30 | max_bodyrate_z: 1.2 # ~ pi*2/3 [rad/s] 31 | min_thrust: 1.0 # ~ 20% gravity [N] 32 | max_thrust: 60.0 # ~ 200% gravity [N] 33 | 34 | # use ending pos for better ending tracking performance) 35 | use_trajectory_ending_pos: true 36 | 37 | # Print information such as timing to terminal 38 | print_info: false 39 | 40 | # control fsm param 41 | ctrl_freq_max: 100.0 42 | 43 | # control fsm param for real-world experiments 44 | max_manual_vel: 1.0 45 | max_angle: 30 # Attitude angle limit in degree. A negative value means no limit. 46 | low_voltage: -0.1 # 4S battery 47 | 48 | filter: 49 | sample_freq_quad_acc: 400.0 50 | sample_freq_quad_omg: 400.0 51 | sample_freq_load_acc: 400.0 52 | sample_freq_load_omg: 400.0 53 | sample_freq_rpm: 400.0 54 | sample_freq_cable: 400.0 55 | sample_freq_dcable: 400.0 56 | 57 | cutoff_freq_quad_acc: 20.0 58 | cutoff_freq_quad_omg: 20.0 #0.0 means no filter 59 | cutoff_freq_load_acc: 20.0 60 | cutoff_freq_load_omg: 20.0 61 | cutoff_freq_rpm: 20.0 62 | cutoff_freq_cable: 20.0 63 | cutoff_freq_dcable: 20.0 64 | 65 | force_estimator: # * 66 | kf: 8.98132e-9 # Also used in the thrust_model 67 | 68 | sample_freq_fq: 400.0 69 | sample_freq_fl: 400.0 70 | cutoff_freq_fq: 0.0 71 | cutoff_freq_fl: 0.0 72 | imu_body_length: 0.66844507 73 | use_force_estimator: true 74 | max_force: 4.0 75 | max_queue: 80 76 | force_observer_freq: 400.0 77 | var_weight: 80.0 78 | 79 | rc_reverse: # * 80 | roll: false 81 | pitch: true 82 | yaw: false 83 | throttle: true 84 | 85 | thrust_model: # The model that maps thrust signal u(0~1) to real thrust force F(Unit:N): F=K1*Voltage^K2*(K3*u^2+(1-K3)*u). 86 | print_value: false # display the value of “thr_scale_compensate” or “hover_percentage” during thrust model estimating. 87 | accurate_thrust_model: 0 88 | # 0 means use the scale factor to map thrust to commmand without online calibration 89 | # 1 means use the scale factor to map thrust to commmand with online calibration(use the rotor speed to calibrate the scale factor) 90 | # 2 means use the scale factor to map thrust to commmand with online calibration(use the IMU calibrate the scale factor. Confilct with the force estimator) 91 | 92 | # approximate thrust mapping parameters 93 | hover_percentage: 0.25 # Thrust percentage in Stabilize/Arco mode WITHOUT Payload 94 | # hover_percentage: 0.3 95 | filter_factor: 0.2 96 | noisy_imu: false # * 97 | 98 | msg_timeout: 99 | odom: 0.5 100 | rc: 0.5 101 | cmd: 0.5 102 | imu: 0.5 103 | bat: 0.5 104 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/EXAMPLES/example1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file EXAMPLES/example1.cpp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Very simple example for testing qpOASES (using QProblem class). 31 | */ 32 | 33 | 34 | #include 35 | 36 | 37 | /** Example for qpOASES main function using the QProblem class. */ 38 | int main( ) 39 | { 40 | /* Setup data of first QP. */ 41 | real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 }; 42 | real_t A[1*2] = { 1.0, 1.0 }; 43 | real_t g[2] = { 1.5, 1.0 }; 44 | real_t lb[2] = { 0.5, -2.0 }; 45 | real_t ub[2] = { 5.0, 2.0 }; 46 | real_t lbA[1] = { -1.0 }; 47 | real_t ubA[1] = { 2.0 }; 48 | 49 | /* Setup data of second QP. */ 50 | real_t g_new[2] = { 1.0, 1.5 }; 51 | real_t lb_new[2] = { 0.0, -1.0 }; 52 | real_t ub_new[2] = { 5.0, -0.5 }; 53 | real_t lbA_new[1] = { -2.0 }; 54 | real_t ubA_new[1] = { 1.0 }; 55 | 56 | 57 | /* Setting up QProblem object. */ 58 | QProblem example( 2,1 ); 59 | 60 | /* Solve first QP. */ 61 | int nWSR = 10; 62 | example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 ); 63 | 64 | /* Solve second QP. */ 65 | nWSR = 10; 66 | example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 ); 67 | 68 | return 0; 69 | } 70 | 71 | 72 | /* 73 | * end of file 74 | */ 75 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/EXAMPLES/example1b.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file EXAMPLES/example1b.cpp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3 28 | * \date 2007-2008 29 | * 30 | * Very simple example for testing qpOASES using the QProblemB class. 31 | */ 32 | 33 | 34 | #include 35 | 36 | 37 | /** Example for qpOASES main function using the QProblemB class. */ 38 | int main( ) 39 | { 40 | /* Setup data of first QP. */ 41 | real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 }; 42 | real_t g[2] = { 1.5, 1.0 }; 43 | real_t lb[2] = { 0.5, -2.0 }; 44 | real_t ub[2] = { 5.0, 2.0 }; 45 | 46 | /* Setup data of second QP. */ 47 | real_t g_new[2] = { 1.0, 1.5 }; 48 | real_t lb_new[2] = { 0.0, -1.0 }; 49 | real_t ub_new[2] = { 5.0, -0.5 }; 50 | 51 | 52 | /* Setting up QProblemB object. */ 53 | QProblemB example( 2 ); 54 | 55 | /* Solve first QP. */ 56 | int nWSR = 10; 57 | example.init( H,g,lb,ub, nWSR,0 ); 58 | 59 | /* Solve second QP. */ 60 | nWSR = 10; 61 | example.hotstart( g_new,lb_new,ub_new, nWSR,0 ); 62 | 63 | return 0; 64 | } 65 | 66 | 67 | /* 68 | * end of file 69 | */ 70 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/INCLUDE/Constants.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file INCLUDE/Constants.hpp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2008 29 | * 30 | * Definition of all global constants. 31 | */ 32 | 33 | 34 | #ifndef QPOASES_CONSTANTS_HPP 35 | #define QPOASES_CONSTANTS_HPP 36 | 37 | #ifndef QPOASES_CUSTOM_INTERFACE 38 | #include "acado_qpoases_interface.hpp" 39 | #else 40 | #define XSTR(x) #x 41 | #define STR(x) XSTR(x) 42 | #include STR(QPOASES_CUSTOM_INTERFACE) 43 | #endif 44 | 45 | /** Maximum number of variables within a QP formulation. 46 | Note: this value has to be positive! */ 47 | const int NVMAX = QPOASES_NVMAX; 48 | 49 | /** Maximum number of constraints within a QP formulation. 50 | Note: this value has to be positive! */ 51 | const int NCMAX = QPOASES_NCMAX; 52 | 53 | /** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays 54 | and compiler errors. */ 55 | const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX; 56 | 57 | /**< Maximum number of working set recalculations. 58 | Note: this value has to be positive! */ 59 | const int NWSRMAX = QPOASES_NWSRMAX; 60 | 61 | /** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is 62 | * issued if this tolerance is not met. 63 | * Note: this value has to be positive! */ 64 | const real_t DESIREDACCURACY = (real_t) 1.0e-3; 65 | 66 | /** Critical KKT tolerance of QP solution; an error is issued if this 67 | * tolerance is not met. 68 | * Note: this value has to be positive! */ 69 | const real_t CRITICALACCURACY = (real_t) 1.0e-2; 70 | 71 | 72 | 73 | /** Numerical value of machine precision (min eps, s.t. 1+eps > 1). 74 | Note: this value has to be positive! */ 75 | const real_t EPS = (real_t) QPOASES_EPS; 76 | 77 | /** Numerical value of zero (for situations in which it would be 78 | * unreasonable to compare with 0.0). 79 | * Note: this value has to be positive! */ 80 | const real_t ZERO = (real_t) 1.0e-50; 81 | 82 | /** Numerical value of infinity (e.g. for non-existing bounds). 83 | * Note: this value has to be positive! */ 84 | const real_t INFTY = (real_t) 1.0e12; 85 | 86 | 87 | /** Lower/upper (constraints') bound tolerance (an inequality constraint 88 | * whose lower and upper bound differ by less than BOUNDTOL is regarded 89 | * to be an equality constraint). 90 | * Note: this value has to be positive! */ 91 | const real_t BOUNDTOL = (real_t) 1.0e-10; 92 | 93 | /** Offset for relaxing (constraints') bounds at beginning of an initial homotopy. 94 | * Note: this value has to be positive! */ 95 | const real_t BOUNDRELAXATION = (real_t) 1.0e3; 96 | 97 | 98 | /** Factor that determines physical lengths of index lists. 99 | * Note: this value has to be greater than 1! */ 100 | const int INDEXLISTFACTOR = 5; 101 | 102 | 103 | #endif /* QPOASES_CONSTANTS_HPP */ 104 | 105 | 106 | /* 107 | * end of file 108 | */ 109 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file INCLUDE/EXTRAS/SolutionAnalysis.hpp 26 | * \author Milan Vukov, Boris Houska, Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2012 29 | * 30 | * Solution analysis class, based on a class in the standard version of the qpOASES 31 | */ 32 | 33 | 34 | // 35 | 36 | #ifndef QPOASES_SOLUTIONANALYSIS_HPP 37 | #define QPOASES_SOLUTIONANALYSIS_HPP 38 | 39 | #include 40 | 41 | /** Enables the computation of variance as is in the standard version of qpOASES */ 42 | #define QPOASES_USE_OLD_VERSION 0 43 | 44 | #if QPOASES_USE_OLD_VERSION 45 | #define KKT_DIM (2 * NVMAX + NCMAX) 46 | #endif 47 | 48 | class SolutionAnalysis 49 | { 50 | public: 51 | 52 | /** Default constructor. */ 53 | SolutionAnalysis( ); 54 | 55 | /** Copy constructor (deep copy). */ 56 | SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */ 57 | ); 58 | 59 | /** Destructor. */ 60 | ~SolutionAnalysis( ); 61 | 62 | /** Copy asingment operator (deep copy). */ 63 | SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */ 64 | ); 65 | 66 | /** A routine for computation of inverse of the Hessian matrix. */ 67 | returnValue getHessianInverse( 68 | QProblem* qp, /** QP */ 69 | real_t* hessianInverse /** Inverse of the Hessian matrix*/ 70 | ); 71 | 72 | /** A routine for computation of inverse of the Hessian matrix. */ 73 | returnValue getHessianInverse( QProblemB* qp, /** QP */ 74 | real_t* hessianInverse /** Inverse of the Hessian matrix*/ 75 | ); 76 | 77 | #if QPOASES_USE_OLD_VERSION 78 | returnValue getVarianceCovariance( 79 | QProblem* qp, 80 | real_t* g_b_bA_VAR, 81 | real_t* Primal_Dual_VAR 82 | ); 83 | #endif 84 | 85 | private: 86 | 87 | real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */ 88 | real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */ 89 | real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */ 90 | real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */ 91 | real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */ 92 | 93 | #if QPOASES_USE_OLD_VERSION 94 | real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */ 95 | #endif 96 | 97 | int FR_idx[ NVMAX ]; /** Index array for free variables */ 98 | int FX_idx[ NVMAX ]; /** Index array for fixed variables */ 99 | int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */ 100 | 101 | real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */ 102 | real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */ 103 | real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */ 104 | real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/ 105 | }; 106 | 107 | #endif // QPOASES_SOLUTIONANALYSIS_HPP 108 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/README.txt: -------------------------------------------------------------------------------- 1 | ## 2 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 3 | ## Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 4 | ## 5 | ## qpOASES is free software; you can redistribute it and/or 6 | ## modify it under the terms of the GNU Lesser General Public 7 | ## License as published by the Free Software Foundation; either 8 | ## version 2.1 of the License, or (at your option) any later version. 9 | ## 10 | ## qpOASES 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 GNU 13 | ## Lesser General Public License for more details. 14 | ## 15 | ## You should have received a copy of the GNU Lesser General Public 16 | ## License along with qpOASES; if not, write to the Free Software 17 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 18 | ## 19 | 20 | 21 | 22 | INTRODUCTION 23 | ============= 24 | 25 | qpOASES is an open-source C++ implementation of the recently proposed 26 | online active set strategy (see [1], [2]), which was inspired by important 27 | observations from the field of parametric quadratic programming. It has 28 | several theoretical features that make it particularly suited for model 29 | predictive control (MPC) applications. 30 | 31 | The software package qpOASES implements these ideas and has already been 32 | successfully used for closed-loop control of a real-world Diesel engine [3]. 33 | 34 | 35 | References: 36 | 37 | [1] H.J. Ferreau. An Online Active Set Strategy for Fast Solution of 38 | Parametric Quadratic Programs with Applications to Predictive Engine Control. 39 | Diplom thesis, University of Heidelberg, 2006. 40 | 41 | [2] H.J. Ferreau, H.G. Bock, M. Diehl. An online active set strategy to 42 | overcome the limitations of explicit MPC. International Journal of Robust 43 | and Nonlinear Control, 18 (8), pp. 816-830, 2008. 44 | 45 | [3] H.J. Ferreau, P. Ortner, P. Langthaler, L. del Re, M. Diehl. Predictive 46 | Control of a Real-World Diesel Engine using an Extended Online Active Set 47 | Strategy. Annual Reviews in Control, 31 (2), pp. 293-301, 2007. 48 | 49 | 50 | 51 | GETTING STARTED 52 | ================ 53 | 54 | 1. For installation, usage and additional information on this software package 55 | see the qpOASES User's Manual located at ./DOC/manual.pdf! 56 | 57 | 58 | 2. The file ./LICENSE.txt contains a copy of the GNU Lesser General Public 59 | License. Please read it carefully before using qpOASES! 60 | 61 | 62 | 3. The whole software package can be downloaded from 63 | 64 | http://homes.esat.kuleuven.be/~optec/software/qpOASES/ 65 | 66 | On this webpage you will also find a list of frequently asked questions. 67 | 68 | 69 | 70 | CONTACT THE AUTHORS 71 | ==================== 72 | 73 | If you have got questions, remarks or comments on qpOASES 74 | please contact the main author: 75 | 76 | Hans Joachim Ferreau 77 | Katholieke Universiteit Leuven 78 | Department of Electrical Engineering (ESAT) 79 | Kasteelpark Arenberg 10, bus 2446 80 | B-3001 Leuven-Heverlee, Belgium 81 | 82 | Phone: +32 16 32 03 63 83 | E-mail: joachim.ferreau@esat.kuleuven.be 84 | qpOASES@esat.kuleuven.be 85 | 86 | Also bug reports and source code extensions are most welcome! 87 | 88 | 89 | 90 | ## 91 | ## end of file 92 | ## 93 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/Bounds.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/Bounds.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Implementation of inlined member functions of the Bounds class designed 31 | * to manage working sets of bounds within a QProblem. 32 | */ 33 | 34 | 35 | /***************************************************************************** 36 | * P U B L I C * 37 | *****************************************************************************/ 38 | 39 | /* 40 | * g e t N V 41 | */ 42 | inline int Bounds::getNV( ) const 43 | { 44 | return nV; 45 | } 46 | 47 | 48 | /* 49 | * g e t N F X 50 | */ 51 | inline int Bounds::getNFV( ) const 52 | { 53 | return nFV; 54 | } 55 | 56 | 57 | /* 58 | * g e t N B V 59 | */ 60 | inline int Bounds::getNBV( ) const 61 | { 62 | return nBV; 63 | } 64 | 65 | 66 | /* 67 | * g e t N U V 68 | */ 69 | inline int Bounds::getNUV( ) const 70 | { 71 | return nUV; 72 | } 73 | 74 | 75 | 76 | /* 77 | * s e t N F X 78 | */ 79 | inline returnValue Bounds::setNFV( int n ) 80 | { 81 | nFV = n; 82 | return SUCCESSFUL_RETURN; 83 | } 84 | 85 | 86 | /* 87 | * s e t N B V 88 | */ 89 | inline returnValue Bounds::setNBV( int n ) 90 | { 91 | nBV = n; 92 | return SUCCESSFUL_RETURN; 93 | } 94 | 95 | 96 | /* 97 | * s e t N U V 98 | */ 99 | inline returnValue Bounds::setNUV( int n ) 100 | { 101 | nUV = n; 102 | return SUCCESSFUL_RETURN; 103 | } 104 | 105 | 106 | /* 107 | * g e t N F R 108 | */ 109 | inline int Bounds::getNFR( ) 110 | { 111 | return free.getLength( ); 112 | } 113 | 114 | 115 | /* 116 | * g e t N F X 117 | */ 118 | inline int Bounds::getNFX( ) 119 | { 120 | return fixed.getLength( ); 121 | } 122 | 123 | 124 | /* 125 | * g e t F r e e 126 | */ 127 | inline Indexlist* Bounds::getFree( ) 128 | { 129 | return &free; 130 | } 131 | 132 | 133 | /* 134 | * g e t F i x e d 135 | */ 136 | inline Indexlist* Bounds::getFixed( ) 137 | { 138 | return &fixed; 139 | } 140 | 141 | 142 | /* 143 | * end of file 144 | */ 145 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/Constraints.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/Constraints.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Declaration of inlined member functions of the Constraints class designed 31 | * to manage working sets of constraints within a QProblem. 32 | */ 33 | 34 | 35 | 36 | /***************************************************************************** 37 | * P U B L I C * 38 | *****************************************************************************/ 39 | 40 | /* 41 | * g e t N C 42 | */ 43 | inline int Constraints::getNC( ) const 44 | { 45 | return nC; 46 | } 47 | 48 | 49 | /* 50 | * g e t N E C 51 | */ 52 | inline int Constraints::getNEC( ) const 53 | { 54 | return nEC; 55 | } 56 | 57 | 58 | /* 59 | * g e t N I C 60 | */ 61 | inline int Constraints::getNIC( ) const 62 | { 63 | return nIC; 64 | } 65 | 66 | 67 | /* 68 | * g e t N U C 69 | */ 70 | inline int Constraints::getNUC( ) const 71 | { 72 | return nUC; 73 | } 74 | 75 | 76 | /* 77 | * s e t N E C 78 | */ 79 | inline returnValue Constraints::setNEC( int n ) 80 | { 81 | nEC = n; 82 | return SUCCESSFUL_RETURN; 83 | } 84 | 85 | 86 | /* 87 | * s e t N I C 88 | */ 89 | inline returnValue Constraints::setNIC( int n ) 90 | { 91 | nIC = n; 92 | return SUCCESSFUL_RETURN; 93 | } 94 | 95 | 96 | /* 97 | * s e t N U C 98 | */ 99 | inline returnValue Constraints::setNUC( int n ) 100 | { 101 | nUC = n; 102 | return SUCCESSFUL_RETURN; 103 | } 104 | 105 | 106 | /* 107 | * g e t N A C 108 | */ 109 | inline int Constraints::getNAC( ) 110 | { 111 | return active.getLength( ); 112 | } 113 | 114 | 115 | /* 116 | * g e t N I A C 117 | */ 118 | inline int Constraints::getNIAC( ) 119 | { 120 | return inactive.getLength( ); 121 | } 122 | 123 | 124 | /* 125 | * g e t A c t i v e 126 | */ 127 | inline Indexlist* Constraints::getActive( ) 128 | { 129 | return &active; 130 | } 131 | 132 | 133 | /* 134 | * g e t I n a c t i v e 135 | */ 136 | inline Indexlist* Constraints::getInactive( ) 137 | { 138 | return &inactive; 139 | } 140 | 141 | 142 | /* 143 | * end of file 144 | */ 145 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/CyclingManager.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/CyclingManager.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Implementation of inlined member functions of the CyclingManager class 31 | * designed to detect and handle possible cycling during QP iterations. 32 | * 33 | */ 34 | 35 | 36 | /***************************************************************************** 37 | * P U B L I C * 38 | *****************************************************************************/ 39 | 40 | /* 41 | * i s C y c l i n g D e t e c t e d 42 | */ 43 | inline BooleanType CyclingManager::isCyclingDetected( ) const 44 | { 45 | return cyclingDetected; 46 | } 47 | 48 | 49 | /* 50 | * end of file 51 | */ 52 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/Indexlist.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/Indexlist.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Implementation of inlined member functions of the Indexlist class designed 31 | * to manage index lists of constraints and bounds within a QProblem_SubjectTo. 32 | */ 33 | 34 | 35 | 36 | /***************************************************************************** 37 | * P U B L I C * 38 | *****************************************************************************/ 39 | 40 | /* 41 | * g e t N u m b e r 42 | */ 43 | inline int Indexlist::getNumber( int physicalindex ) const 44 | { 45 | /* consistency check */ 46 | if ( ( physicalindex < 0 ) || ( physicalindex > length ) ) 47 | return -RET_INDEXLIST_OUTOFBOUNDS; 48 | 49 | return number[physicalindex]; 50 | } 51 | 52 | 53 | /* 54 | * g e t L e n g t h 55 | */ 56 | inline int Indexlist::getLength( ) 57 | { 58 | return length; 59 | } 60 | 61 | 62 | /* 63 | * g e t L a s t N u m b e r 64 | */ 65 | inline int Indexlist::getLastNumber( ) const 66 | { 67 | return number[last]; 68 | } 69 | 70 | 71 | /* 72 | * g e t L a s t N u m b e r 73 | */ 74 | inline BooleanType Indexlist::isMember( int _number ) const 75 | { 76 | if ( getIndex( _number ) >= 0 ) 77 | return BT_TRUE; 78 | else 79 | return BT_FALSE; 80 | } 81 | 82 | 83 | /* 84 | * end of file 85 | */ 86 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/MessageHandling.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/MessageHandling.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Implementation of inlined member functions of the MessageHandling class. 31 | */ 32 | 33 | 34 | 35 | /***************************************************************************** 36 | * P U B L I C * 37 | *****************************************************************************/ 38 | 39 | /* 40 | * g e t E r r o r V i s i b i l i t y S t a t u s 41 | */ 42 | inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const 43 | { 44 | return errorVisibility; 45 | } 46 | 47 | 48 | /* 49 | * g e t W a r n i n g V i s i b i l i t y S t a t u s 50 | */ 51 | inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const 52 | { 53 | return warningVisibility; 54 | } 55 | 56 | 57 | /* 58 | * g e t I n f o V i s i b i l i t y S t a t u s 59 | */ 60 | inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const 61 | { 62 | return infoVisibility; 63 | } 64 | 65 | 66 | /* 67 | * g e t O u t p u t F i l e 68 | */ 69 | inline myFILE* MessageHandling::getOutputFile( ) const 70 | { 71 | return outputFile; 72 | } 73 | 74 | 75 | /* 76 | * g e t E r r o r C o u n t 77 | */ 78 | inline int MessageHandling::getErrorCount( ) const 79 | { 80 | return errorCount; 81 | } 82 | 83 | 84 | /* 85 | * s e t E r r o r V i s i b i l i t y S t a t u s 86 | */ 87 | inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility ) 88 | { 89 | errorVisibility = _errorVisibility; 90 | } 91 | 92 | 93 | /* 94 | * s e t W a r n i n g V i s i b i l i t y S t a t u s 95 | */ 96 | inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility ) 97 | { 98 | warningVisibility = _warningVisibility; 99 | } 100 | 101 | 102 | /* 103 | * s e t I n f o V i s i b i l i t y S t a t u s 104 | */ 105 | inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility ) 106 | { 107 | infoVisibility = _infoVisibility; 108 | } 109 | 110 | 111 | /* 112 | * s e t O u t p u t F i l e 113 | */ 114 | inline void MessageHandling::setOutputFile( myFILE* _outputFile ) 115 | { 116 | outputFile = _outputFile; 117 | } 118 | 119 | 120 | /* 121 | * s e t E r r o r C o u n t 122 | */ 123 | inline returnValue MessageHandling::setErrorCount( int _errorCount ) 124 | { 125 | if ( _errorCount >= 0 ) 126 | { 127 | errorCount = _errorCount; 128 | return SUCCESSFUL_RETURN; 129 | } 130 | else 131 | return RET_INVALID_ARGUMENTS; 132 | } 133 | 134 | 135 | /* 136 | * end of file 137 | */ 138 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/SubjectTo.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | /** 25 | * \file SRC/SubjectTo.ipp 26 | * \author Hans Joachim Ferreau 27 | * \version 1.3embedded 28 | * \date 2007-2008 29 | * 30 | * Implementation of the inlined member functions of the SubjectTo class 31 | * designed to manage working sets of constraints and bounds within a QProblem. 32 | */ 33 | 34 | 35 | /***************************************************************************** 36 | * P U B L I C * 37 | *****************************************************************************/ 38 | 39 | 40 | /* 41 | * g e t T y p e 42 | */ 43 | inline SubjectToType SubjectTo::getType( int i ) const 44 | { 45 | if ( ( i >= 0 ) && ( i < size ) ) 46 | return type[i]; 47 | else 48 | return ST_UNKNOWN; 49 | } 50 | 51 | 52 | /* 53 | * g e t S t a t u s 54 | */ 55 | inline SubjectToStatus SubjectTo::getStatus( int i ) const 56 | { 57 | if ( ( i >= 0 ) && ( i < size ) ) 58 | return status[i]; 59 | else 60 | return ST_UNDEFINED; 61 | } 62 | 63 | 64 | /* 65 | * s e t T y p e 66 | */ 67 | inline returnValue SubjectTo::setType( int i, SubjectToType value ) 68 | { 69 | if ( ( i >= 0 ) && ( i < size ) ) 70 | { 71 | type[i] = value; 72 | return SUCCESSFUL_RETURN; 73 | } 74 | else 75 | return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); 76 | } 77 | 78 | 79 | /* 80 | * s e t S t a t u s 81 | */ 82 | inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value ) 83 | { 84 | if ( ( i >= 0 ) && ( i < size ) ) 85 | { 86 | status[i] = value; 87 | return SUCCESSFUL_RETURN; 88 | } 89 | else 90 | return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); 91 | } 92 | 93 | 94 | /* 95 | * s e t N o L o w e r 96 | */ 97 | inline void SubjectTo::setNoLower( BooleanType _status ) 98 | { 99 | noLower = _status; 100 | } 101 | 102 | 103 | /* 104 | * s e t N o U p p e r 105 | */ 106 | inline void SubjectTo::setNoUpper( BooleanType _status ) 107 | { 108 | noUpper = _status; 109 | } 110 | 111 | 112 | /* 113 | * i s N o L o w e r 114 | */ 115 | inline BooleanType SubjectTo::isNoLower( ) const 116 | { 117 | return noLower; 118 | } 119 | 120 | 121 | /* 122 | * i s N o L o w e r 123 | */ 124 | inline BooleanType SubjectTo::isNoUpper( ) const 125 | { 126 | return noUpper; 127 | } 128 | 129 | 130 | /* 131 | * end of file 132 | */ 133 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/SRC/Utils.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 6 | * 7 | * qpOASES is free software; you can redistribute it and/or 8 | * modify it under the terms of the GNU Lesser General Public 9 | * License as published by the Free Software Foundation; either 10 | * version 2.1 of the License, or (at your option) any later version. 11 | * 12 | * qpOASES is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public 18 | * License along with qpOASES; if not, write to the Free Software 19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | * 21 | */ 22 | 23 | 24 | 25 | /** 26 | * \file SRC/Utils.ipp 27 | * \author Hans Joachim Ferreau 28 | * \version 1.3embedded 29 | * \date 2007-2008 30 | * 31 | * Implementation of some inlined utilities for working with the different QProblem 32 | * classes. 33 | */ 34 | 35 | 36 | 37 | /* 38 | * g e t A b s 39 | */ 40 | inline real_t getAbs( real_t x ) 41 | { 42 | if ( x < 0.0 ) 43 | return -x; 44 | else 45 | return x; 46 | } 47 | 48 | 49 | /* 50 | * end of file 51 | */ 52 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/externals/qpoases/VERSIONS.txt: -------------------------------------------------------------------------------- 1 | ## 2 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 3 | ## Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. 4 | ## 5 | ## qpOASES is free software; you can redistribute it and/or 6 | ## modify it under the terms of the GNU Lesser General Public 7 | ## License as published by the Free Software Foundation; either 8 | ## version 2.1 of the License, or (at your option) any later version. 9 | ## 10 | ## qpOASES 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 GNU 13 | ## Lesser General Public License for more details. 14 | ## 15 | ## You should have received a copy of the GNU Lesser General Public 16 | ## License along with qpOASES; if not, write to the Free Software 17 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 18 | ## 19 | 20 | 21 | 22 | VERSION HISTORY 23 | =============== 24 | 25 | 1.3embedded (last updated on 30th April 2009): 26 | ----------------------------------------------------------------------- 27 | 28 | + Re-programming of internal memory management to avoid dynamic memory allocations 29 | + Most #ifdef directives removed 30 | + Almost all type definitions gathered within INCLUDE/Types.hpp 31 | + Irrelevant functionality removed (like the SQProblem class, functionality 32 | for loading data from files or the SCILAB interface) 33 | + Replacement of all doubles by real_t 34 | + Introduction of define "PC_DEBUG" for switching off all print functions 35 | + stdio.h was made optional, string.h is no longer needed 36 | + relative paths removed from #include directives 37 | + made auxiliary objects locally static within solveInitialQP() 38 | + Matlab interface fixed for single precision 39 | + New return value -2 from Legacy wrapper added to Matlab/Simulink interfaces 40 | + KKT optimality check moved into QProblem(B) class, SolutionAnalysis class removed 41 | 42 | 43 | 1.3 (released on 2nd June 2008, last updated on 19th June 2008): 44 | ----------------------------------------------------------------------- 45 | 46 | + Implementation of "initialised homotopy" concept 47 | + Addition of the SolutionAnalysis class 48 | + Utility functions for solving test problems in OQP format added 49 | + Flexibility of Matlab(R) interface enhanced 50 | + Major source code cleanup 51 | (Attention: a few class names and calling interfaces have changed!) 52 | 53 | 54 | 55 | 1.2 (released on 9th October 2007): 56 | ----------------------------------------------------------------------- 57 | 58 | + Special treatment of diagonal Hessians 59 | + Improved infeasibility detection 60 | + Further improved Matlab(R) interface 61 | + Extended Simulink(R) interface 62 | + scilab interface added 63 | + Code cleanup and several bugfixes 64 | 65 | 66 | 67 | 1.1 (released on 8th July 2007): 68 | -------------------------------- 69 | 70 | + Implementation of the QProblemB class 71 | + Basic implementation of the SQProblem class 72 | + Improved Matlab(R) interface 73 | + Enabling/Disabling of constraints introduced 74 | + Several bugfixes 75 | 76 | 77 | 78 | 1.0 (released on 17th April 2007): 79 | ---------------------------------- 80 | 81 | Initial release. 82 | 83 | 84 | 85 | ## 86 | ## end of file 87 | ## 88 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required( VERSION 2.8.3) 2 | 3 | project( quadrotor_payload_mpc ) 4 | 5 | set( CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR} ) 6 | add_compile_options(-std=c++14) 7 | 8 | # Prerequisites 9 | find_package( ACADO REQUIRED ) 10 | 11 | # Include directories 12 | include_directories( . ${ACADO_INCLUDE_DIRS} ) 13 | 14 | 15 | # Build an executable 16 | add_executable( quadrotor_payload_mpc_with_ext_force_codegen quadrotor_payload_mpc_with_ext_force.cpp Yaml.cpp) 17 | target_link_libraries( quadrotor_payload_mpc_with_ext_force_codegen ${ACADO_SHARED_LIBRARIES} ) 18 | set_target_properties( quadrotor_payload_mpc_with_ext_force_codegen PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) 19 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/model/README.md: -------------------------------------------------------------------------------- 1 | # How to compile the MPC code generation 2 | 3 | 1. Install ACADO as described in http://acado.github.io/install_linux.html 4 | 5 | 2. Source the file ACADO_ROOT/build/acdo_env.sh 6 | 7 | 3. Go into payload_mpc_controller/model 8 | 9 | 4. Prepare the make with "cmake ." 10 | 11 | 5. Compile it with "make" 12 | 13 | 6. Modify the parameters in "config/model.yaml" to match your model 14 | 15 | 7. Run the executable to generate all code "./quadrotor_payload_mpc_with_ext_force_codegen ../config/model.yaml" 16 | 17 | (You can specify the model parameter file path) 18 | 19 | Thanks to [rpg_mpc](https://github.com/uzh-rpg/rpg_mpc) 20 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/model/quadrotor_payload_mpc/acado_auxiliary_functions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was auto-generated using the ACADO Toolkit. 3 | * 4 | * While ACADO Toolkit is free software released under the terms of 5 | * the GNU Lesser General Public License (LGPL), the generated code 6 | * as such remains the property of the user who used ACADO Toolkit 7 | * to generate this code. In particular, user dependent data of the code 8 | * do not inherit the GNU LGPL license. On the other hand, parts of the 9 | * generated code that are a direct copy of source code from the 10 | * ACADO Toolkit or the software tools it is based on, remain, as derived 11 | * work, automatically covered by the LGPL license. 12 | * 13 | * ACADO Toolkit is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * 17 | */ 18 | 19 | 20 | #ifndef ACADO_AUXILIARY_FUNCTIONS_H 21 | #define ACADO_AUXILIARY_FUNCTIONS_H 22 | 23 | #include "acado_common.h" 24 | 25 | #ifndef __MATLAB__ 26 | #ifdef __cplusplus 27 | extern "C" 28 | { 29 | #endif /* __cplusplus */ 30 | #endif /* __MATLAB__ */ 31 | 32 | /** Get pointer to the matrix with differential variables. */ 33 | real_t* acado_getVariablesX( ); 34 | 35 | /** Get pointer to the matrix with control variables. */ 36 | real_t* acado_getVariablesU( ); 37 | 38 | #if ACADO_NY > 0 39 | /** Get pointer to the matrix with references/measurements. */ 40 | real_t* acado_getVariablesY( ); 41 | #endif 42 | 43 | #if ACADO_NYN > 0 44 | /** Get pointer to the vector with references/measurement on the last node. */ 45 | real_t* acado_getVariablesYN( ); 46 | #endif 47 | 48 | /** Get pointer to the current state feedback vector. Only applicable for NMPC. */ 49 | real_t* acado_getVariablesX0( ); 50 | 51 | /** Print differential variables. */ 52 | void acado_printDifferentialVariables( ); 53 | 54 | /** Print control variables. */ 55 | void acado_printControlVariables( ); 56 | 57 | /** Print ACADO code generation notice. */ 58 | void acado_printHeader( ); 59 | 60 | /* 61 | * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 62 | * providing us with the following timing routines. 63 | */ 64 | 65 | #if !(defined _DSPACE) 66 | #if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) 67 | 68 | /* Use Windows QueryPerformanceCounter for timing. */ 69 | #include 70 | 71 | /** A structure for keeping internal timer data. */ 72 | typedef struct acado_timer_ 73 | { 74 | LARGE_INTEGER tic; 75 | LARGE_INTEGER toc; 76 | LARGE_INTEGER freq; 77 | } acado_timer; 78 | 79 | 80 | #elif (defined __APPLE__) 81 | 82 | #include "unistd.h" 83 | #include 84 | 85 | /** A structure for keeping internal timer data. */ 86 | typedef struct acado_timer_ 87 | { 88 | uint64_t tic; 89 | uint64_t toc; 90 | mach_timebase_info_data_t tinfo; 91 | } acado_timer; 92 | 93 | #else 94 | 95 | /* Use POSIX clock_gettime() for timing on non-Windows machines. */ 96 | #include 97 | 98 | #if __STDC_VERSION__ >= 199901L 99 | /* C99 mode of operation. */ 100 | 101 | #include 102 | #include 103 | 104 | typedef struct acado_timer_ 105 | { 106 | struct timeval tic; 107 | struct timeval toc; 108 | } acado_timer; 109 | 110 | #else 111 | /* ANSI C */ 112 | 113 | /** A structure for keeping internal timer data. */ 114 | typedef struct acado_timer_ 115 | { 116 | struct timespec tic; 117 | struct timespec toc; 118 | } acado_timer; 119 | 120 | #endif /* __STDC_VERSION__ >= 199901L */ 121 | 122 | #endif /* (defined _WIN32 || defined _WIN64) */ 123 | 124 | /** A function for measurement of the current time. */ 125 | void acado_tic( acado_timer* t ); 126 | 127 | /** A function which returns the elapsed time. */ 128 | real_t acado_toc( acado_timer* t ); 129 | 130 | #endif 131 | 132 | #ifndef __MATLAB__ 133 | #ifdef __cplusplus 134 | } /* extern "C" */ 135 | #endif /* __cplusplus */ 136 | #endif /* __MATLAB__ */ 137 | 138 | #endif /* ACADO_AUXILIARY_FUNCTIONS_H */ 139 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/model/quadrotor_payload_mpc/acado_qpoases_interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was auto-generated using the ACADO Toolkit. 3 | * 4 | * While ACADO Toolkit is free software released under the terms of 5 | * the GNU Lesser General Public License (LGPL), the generated code 6 | * as such remains the property of the user who used ACADO Toolkit 7 | * to generate this code. In particular, user dependent data of the code 8 | * do not inherit the GNU LGPL license. On the other hand, parts of the 9 | * generated code that are a direct copy of source code from the 10 | * ACADO Toolkit or the software tools it is based on, remain, as derived 11 | * work, automatically covered by the LGPL license. 12 | * 13 | * ACADO Toolkit is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * 17 | */ 18 | 19 | 20 | extern "C" 21 | { 22 | #include "acado_common.h" 23 | } 24 | 25 | #include "INCLUDE/QProblemB.hpp" 26 | 27 | #if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 28 | #include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" 29 | #endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ 30 | 31 | static int acado_nWSR; 32 | 33 | 34 | 35 | #if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 36 | static SolutionAnalysis acado_sa; 37 | #endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ 38 | 39 | int acado_solve( void ) 40 | { 41 | acado_nWSR = QPOASES_NWSRMAX; 42 | 43 | QProblemB qp( 80 ); 44 | 45 | returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.lb, acadoWorkspace.ub, acado_nWSR, acadoWorkspace.y); 46 | 47 | qp.getPrimalSolution( acadoWorkspace.x ); 48 | qp.getDualSolution( acadoWorkspace.y ); 49 | 50 | #if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 51 | 52 | if (retVal != SUCCESSFUL_RETURN) 53 | return (int)retVal; 54 | 55 | retVal = acado_sa.getHessianInverse( &qp, ); 56 | 57 | #endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ 58 | 59 | return (int)retVal; 60 | } 61 | 62 | int acado_getNWSR( void ) 63 | { 64 | return acado_nWSR; 65 | } 66 | 67 | const char* acado_getErrorString( int error ) 68 | { 69 | return MessageHandling::getErrorString( error ); 70 | } 71 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/model/quadrotor_payload_mpc/acado_qpoases_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was auto-generated using the ACADO Toolkit. 3 | * 4 | * While ACADO Toolkit is free software released under the terms of 5 | * the GNU Lesser General Public License (LGPL), the generated code 6 | * as such remains the property of the user who used ACADO Toolkit 7 | * to generate this code. In particular, user dependent data of the code 8 | * do not inherit the GNU LGPL license. On the other hand, parts of the 9 | * generated code that are a direct copy of source code from the 10 | * ACADO Toolkit or the software tools it is based on, remain, as derived 11 | * work, automatically covered by the LGPL license. 12 | * 13 | * ACADO Toolkit is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * 17 | */ 18 | 19 | 20 | #ifndef QPOASES_HEADER 21 | #define QPOASES_HEADER 22 | 23 | #ifdef PC_DEBUG 24 | #include 25 | #endif /* PC_DEBUG */ 26 | 27 | #include 28 | 29 | #ifdef __cplusplus 30 | #define EXTERNC extern "C" 31 | #else 32 | #define EXTERNC 33 | #endif 34 | 35 | /* 36 | * A set of options for qpOASES 37 | */ 38 | 39 | /** Maximum number of optimization variables. */ 40 | #define QPOASES_NVMAX 80 41 | /** Maximum number of constraints. */ 42 | #define QPOASES_NCMAX 0 43 | /** Maximum number of working set recalculations. */ 44 | #define QPOASES_NWSRMAX 240 45 | /** Print level for qpOASES. */ 46 | #define QPOASES_PRINTLEVEL PL_NONE 47 | /** The value of EPS */ 48 | #define QPOASES_EPS 1.193e-07 49 | /** Internally used floating point type */ 50 | typedef float real_t; 51 | 52 | /* 53 | * Forward function declarations 54 | */ 55 | 56 | /** A function that calls the QP solver */ 57 | EXTERNC int acado_solve( void ); 58 | 59 | /** Get the number of active set changes */ 60 | EXTERNC int acado_getNWSR( void ); 61 | 62 | /** Get the error string. */ 63 | const char* acado_getErrorString( int error ); 64 | 65 | #endif /* QPOASES_HEADER */ 66 | -------------------------------------------------------------------------------- /controller/payload_mpc_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | payload_mpc_controller 4 | 0.0.0 5 | Model Predictive Control for Quadrotors with Payload 6 | 7 | Haojia Li 8 | 9 | GNU GPL 10 | 11 | catkin 12 | mav_msgs 13 | autopilot 14 | geometry_msgs 15 | quadrotor_msgs 16 | roscpp 17 | std_msgs 18 | trajectory_msgs 19 | quadrotor_common 20 | sensor_msgs 21 | uav_utils 22 | mavros 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /imgs/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/imgs/cover.png -------------------------------------------------------------------------------- /imgs/simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/imgs/simulation.gif -------------------------------------------------------------------------------- /imgs/system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/imgs/system.png -------------------------------------------------------------------------------- /planner/path_searching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(path_searching) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++17 ) 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native") 7 | 8 | find_package(Eigen3 REQUIRED) 9 | find_package(PCL 1.7 REQUIRED) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | visualization_msgs 16 | plan_env 17 | cv_bridge 18 | ) 19 | 20 | 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES path_searching 24 | CATKIN_DEPENDS plan_env 25 | # DEPENDS system_lib 26 | ) 27 | 28 | include_directories( 29 | SYSTEM 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ${Eigen3_INCLUDE_DIRS} 33 | ${PCL_INCLUDE_DIRS} 34 | ) 35 | 36 | add_library( path_searching 37 | src/dyn_a_star.cpp 38 | src/kinodynamic_astar.cpp 39 | ) 40 | target_link_libraries( path_searching 41 | ${catkin_LIBRARIES} 42 | ) 43 | -------------------------------------------------------------------------------- /planner/path_searching/include/path_searching/dyn_a_star.h: -------------------------------------------------------------------------------- 1 | #ifndef _DYN_A_STAR_H_ 2 | #define _DYN_A_STAR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | constexpr double inf = 1 >> 20; 12 | struct GridNode; 13 | typedef GridNode *GridNodePtr; 14 | 15 | struct GridNode 16 | { 17 | enum enum_state 18 | { 19 | OPENSET = 1, 20 | CLOSEDSET = 2, 21 | UNDEFINED = 3 22 | }; 23 | 24 | int rounds{0}; // Distinguish every call 25 | enum enum_state state 26 | { 27 | UNDEFINED 28 | }; 29 | Eigen::Vector3i index; 30 | 31 | double gScore{inf}, fScore{inf}; 32 | GridNodePtr cameFrom{NULL}; 33 | }; 34 | 35 | class NodeComparator 36 | { 37 | public: 38 | bool operator()(GridNodePtr node1, GridNodePtr node2) 39 | { 40 | return node1->fScore > node2->fScore; 41 | } 42 | }; 43 | 44 | class AStar 45 | { 46 | private: 47 | GridMap::Ptr grid_map_; 48 | 49 | inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z); 50 | 51 | double getDiagHeu(GridNodePtr node1, GridNodePtr node2); 52 | double getManhHeu(GridNodePtr node1, GridNodePtr node2); 53 | double getEuclHeu(GridNodePtr node1, GridNodePtr node2); 54 | inline double getHeu(GridNodePtr node1, GridNodePtr node2); 55 | 56 | bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx); 57 | 58 | inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const; 59 | inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const; 60 | 61 | //bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos ); 62 | 63 | inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); } 64 | inline bool checkOccupancy_esdf(const Eigen::Vector3d &pos){ 65 | const double dist = 0.2; 66 | if (grid_map_->getDistance(pos) < dist ) 67 | return true; 68 | else 69 | return false; 70 | } 71 | 72 | // inline bool checkOccupancy(const Eigen::Vector3d &pos) { 73 | // const double dist = 0.2; 74 | // if (grid_map_->getDistance(pos) < dist ) 75 | // return true; 76 | // else 77 | // return false; 78 | // } 79 | 80 | std::vector retrievePath(GridNodePtr current); 81 | 82 | double step_size_, inv_step_size_; 83 | Eigen::Vector3d center_; 84 | Eigen::Vector3i CENTER_IDX_, POOL_SIZE_; 85 | const double tie_breaker_ = 1.0 + 1.0 / 10000; 86 | 87 | std::vector gridPath_; 88 | 89 | GridNodePtr ***GridNodeMap_; 90 | std::priority_queue, NodeComparator> openSet_; 91 | 92 | int rounds_{0}; 93 | 94 | public: 95 | typedef std::shared_ptr Ptr; 96 | 97 | AStar(){}; 98 | ~AStar(); 99 | 100 | void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size); 101 | 102 | bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt, bool use_esdf_check); 103 | 104 | std::vector getPath(); 105 | 106 | std::vector astarSearchAndGetSimplePath(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); 107 | }; 108 | 109 | inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2) 110 | { 111 | return tie_breaker_ * getDiagHeu(node1, node2); 112 | } 113 | 114 | inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const 115 | { 116 | return ((index - CENTER_IDX_).cast() * step_size_) + center_; 117 | }; 118 | 119 | inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const 120 | { 121 | idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast() + CENTER_IDX_; 122 | 123 | if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2)) 124 | { 125 | ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2)); 126 | return false; 127 | } 128 | 129 | return true; 130 | }; 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /planner/path_searching/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_searching 4 | 0.0.0 5 | The path_searching package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | plan_env 56 | roscpp 57 | rospy 58 | std_msgs 59 | plan_env 60 | roscpp 61 | rospy 62 | std_msgs 63 | plan_env 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /planner/plan_env/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(plan_env) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++17 ) 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native") 7 | 8 | find_package(OpenCV REQUIRED) 9 | 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | visualization_msgs 16 | cv_bridge 17 | message_filters 18 | ) 19 | 20 | find_package(Eigen3 REQUIRED) 21 | find_package(PCL 1.7 REQUIRED) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS include 25 | LIBRARIES plan_env 26 | CATKIN_DEPENDS roscpp std_msgs 27 | # DEPENDS system_lib 28 | ) 29 | 30 | include_directories( 31 | SYSTEM 32 | include 33 | ${catkin_INCLUDE_DIRS} 34 | ${Eigen3_INCLUDE_DIRS} 35 | ${PCL_INCLUDE_DIRS} 36 | ${OpenCV_INCLUDE_DIRS} 37 | ) 38 | 39 | link_directories(${PCL_LIBRARY_DIRS}) 40 | 41 | add_library( plan_env 42 | src/grid_map.cpp 43 | src/raycast.cpp 44 | ) 45 | target_link_libraries( plan_env 46 | ${catkin_LIBRARIES} 47 | ${PCL_LIBRARIES} 48 | ) 49 | -------------------------------------------------------------------------------- /planner/plan_env/include/plan_env/raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_H_ 2 | #define RAYCAST_H_ 3 | 4 | #include 5 | #include 6 | 7 | double signum(double x); 8 | 9 | double mod(double value, double modulus); 10 | 11 | double intbound(double s, double ds); 12 | 13 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 14 | const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output); 15 | 16 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 17 | const Eigen::Vector3d& max, std::vector* output); 18 | 19 | class RayCaster { 20 | private: 21 | /* data */ 22 | Eigen::Vector3d start_; 23 | Eigen::Vector3d end_; 24 | Eigen::Vector3d direction_; 25 | Eigen::Vector3d min_; 26 | Eigen::Vector3d max_; 27 | int x_; 28 | int y_; 29 | int z_; 30 | int endX_; 31 | int endY_; 32 | int endZ_; 33 | double maxDist_; 34 | double dx_; 35 | double dy_; 36 | double dz_; 37 | int stepX_; 38 | int stepY_; 39 | int stepZ_; 40 | double tMaxX_; 41 | double tMaxY_; 42 | double tMaxZ_; 43 | double tDeltaX_; 44 | double tDeltaY_; 45 | double tDeltaZ_; 46 | double dist_; 47 | 48 | int step_num_; 49 | 50 | public: 51 | RayCaster(/* args */) { 52 | } 53 | ~RayCaster() { 54 | } 55 | 56 | bool setInput(const Eigen::Vector3d& start, 57 | const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, 58 | const Eigen::Vector3d& max */); 59 | 60 | bool step(Eigen::Vector3d& ray_pt); 61 | }; 62 | 63 | #endif // RAYCAST_H_ -------------------------------------------------------------------------------- /planner/plan_env/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | plan_env 4 | 0.0.0 5 | The plan_env package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /planner/plan_manage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(payload_planner) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++17 ) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | find_package(PCL 1.7 REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | quadrotor_msgs 17 | plan_env 18 | path_searching 19 | traj_opt 20 | traj_utils 21 | message_generation 22 | cv_bridge 23 | ) 24 | 25 | # catkin_package(CATKIN_DEPENDS message_runtime) 26 | catkin_package( 27 | INCLUDE_DIRS include 28 | LIBRARIES payload_planner 29 | CATKIN_DEPENDS plan_env path_searching traj_opt traj_utils 30 | # DEPENDS system_lib 31 | ) 32 | 33 | include_directories( 34 | include 35 | SYSTEM 36 | ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include 37 | ${EIGEN3_INCLUDE_DIR} 38 | ${PCL_INCLUDE_DIRS} 39 | ) 40 | 41 | add_library( vis_utils 42 | src/planning_visualization.cpp 43 | ) 44 | target_link_libraries( vis_utils 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | add_executable(payload_planner_node 49 | src/payload_planner_node.cpp 50 | src/replan_fsm.cpp 51 | src/planner_manager.cpp 52 | ) 53 | target_link_libraries(payload_planner_node 54 | ${catkin_LIBRARIES} 55 | vis_utils 56 | ) 57 | 58 | 59 | -------------------------------------------------------------------------------- /planner/plan_manage/config/planning_params.yaml: -------------------------------------------------------------------------------- 1 | optimization: 2 | weight_obstacle : 55000.0 3 | weight_feasibility : 400.0 4 | weight_quad_feasibility : 400.0 5 | weight_sqrvariance : 400.0 6 | weight_time : 500.0 7 | constrain_points_perPiece: 32 8 | 9 | # system Config 10 | payload_size : 0.12 11 | drone_size : 0.18 12 | L_length : 0.66844507 13 | sample_ball_distance : 0.1 14 | safe_margin : 0.2 15 | grav : 9.795 16 | min_thrust : 0.5 17 | max_thrust : 40.0 18 | max_theta : 60.0 19 | mass_payload : 0.283 20 | mass_quad : 1.150 21 | 22 | #kinodynamic path searching 23 | search: 24 | max_tau : 0.6 25 | init_max_tau : 0.8 26 | w_time : 10.0 27 | horizon : 5.0 28 | lambda_heu : 5.0 29 | resolution_astar: 0.1 30 | time_resolution : 0.8 31 | margin : 0.2 32 | allocate_num : 500000 33 | check_num : 8 -------------------------------------------------------------------------------- /planner/plan_manage/include/plan_manage/planner_manager.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace payload_planner 15 | { 16 | 17 | // Planner Manager 18 | // Key algorithms of mapping and planning are called in this class 19 | 20 | class PlannerManager 21 | { 22 | public: 23 | 24 | PlannerManager(); 25 | ~PlannerManager(); 26 | 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | /* main planning interface */ 30 | bool reboundReplan( 31 | const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc, const Eigen::Vector3d &start_jerk , 32 | const double trajectory_start_time, const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel ,const Eigen::Vector3d &end_acc, 33 | const bool flag_polyInit, const bool have_local_traj); 34 | bool computeInitReferenceState( 35 | const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, 36 | const Eigen::Vector3d &start_acc, const Eigen::Vector3d &start_jerk , const Eigen::Vector3d &local_target_pt, 37 | const Eigen::Vector3d &local_target_vel,const Eigen::Vector3d &local_target_acc, const double &ts, poly_traj::MinSnapOpt &initMJO, 38 | const bool flag_polyInit); 39 | bool planGlobalTrajWaypoints( 40 | const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, 41 | const Eigen::Vector3d &start_acc, const std::vector &waypoints, 42 | const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc); 43 | void getLocalTarget( 44 | const double planning_horizen, 45 | const Eigen::Vector3d &start_pt, const Eigen::Vector3d &global_end_pt, 46 | Eigen::Vector3d &local_target_pos, Eigen::Vector3d &local_target_vel,Eigen::Vector3d &local_target_acc); 47 | void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL); 48 | bool EmergencyStop(Eigen::Vector3d stop_pos); 49 | 50 | PlanParameters pp_; 51 | GridMap::Ptr grid_map_; 52 | TrajContainer traj_; 53 | 54 | // ros::Publisher obj_pub_; //zx-todo 55 | 56 | PolyTrajOptimizer::Ptr ploy_traj_opt_; 57 | 58 | bool start_flag_, reach_flag_; 59 | ros::Time global_start_time_; 60 | double start_time_, reach_time_, average_plan_time_; 61 | 62 | private: 63 | /* main planning algorithms & modules */ 64 | PlanningVisualization::Ptr visualization_; 65 | 66 | int continous_failures_count_{0}; 67 | 68 | public: 69 | typedef unique_ptr Ptr; 70 | 71 | // !SECTION 72 | }; 73 | } // namespace payload_planner -------------------------------------------------------------------------------- /planner/plan_manage/include/plan_manage/planning_visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANNING_VISUALIZATION_H_ 2 | #define _PLANNING_VISUALIZATION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | using std::vector; 16 | namespace payload_planner 17 | { 18 | class PlanningVisualization 19 | { 20 | private: 21 | ros::NodeHandle node; 22 | 23 | ros::Publisher goal_point_pub; 24 | ros::Publisher global_list_pub; 25 | ros::Publisher init_list_pub; 26 | ros::Publisher optimal_list_pub; 27 | ros::Publisher failed_list_pub; 28 | ros::Publisher a_star_list_pub; 29 | ros::Publisher guide_vector_pub; 30 | ros::Publisher init_list_debug_pub; 31 | 32 | ros::Publisher intermediate_pt0_pub; 33 | ros::Publisher intermediate_pt1_pub; 34 | ros::Publisher intermediate_grad0_pub; 35 | ros::Publisher intermediate_grad1_pub; 36 | ros::Publisher intermediate_grad_smoo_pub; 37 | ros::Publisher intermediate_grad_dist_pub; 38 | ros::Publisher intermediate_grad_feas_pub; 39 | ros::Publisher ellipsoidPub; 40 | 41 | public: 42 | PlanningVisualization(/* args */) {} 43 | ~PlanningVisualization() 44 | { 45 | } 46 | 47 | PlanningVisualization(ros::NodeHandle &nh); 48 | 49 | typedef std::shared_ptr Ptr; 50 | 51 | void displayMarkerList(ros::Publisher &pub, const vector &list, double scale, 52 | Eigen::Vector4d color, int id, bool show_sphere = true); 53 | void generatePathDisplayArray(visualization_msgs::MarkerArray &array, 54 | const vector &list, double scale, Eigen::Vector4d color, int id); 55 | void generateArrowDisplayArray(visualization_msgs::MarkerArray &array, 56 | const vector &list, double scale, Eigen::Vector4d color, int id); 57 | void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id); 58 | void displayGlobalPathList(vector global_pts, const double scale, int id); 59 | void displayInitPathList(vector init_pts, const double scale, int id); 60 | void displayMultiInitPathList(vector> init_trajs, const double scale); 61 | void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); 62 | void displayFailedList(Eigen::MatrixXd failed_pts, int id); 63 | void displayAStarList(std::vector> a_star_paths, int id); 64 | void displayArrowList(ros::Publisher &pub, const vector &list, double scale, Eigen::Vector4d color, int id); 65 | void displayInitPathListDebug(vector init_pts, const double scale, int id); 66 | 67 | void displayIntermediatePt(std::string type, Eigen::MatrixXd &pts, int id, Eigen::Vector4d color); 68 | void displayIntermediateGrad(std::string type, Eigen::MatrixXd &pts, Eigen::MatrixXd &grad, int id, Eigen::Vector4d color); 69 | void visualizeDoubleball(const poly_traj::Trajectory<7> &traj, int samples = 3, double L = 0.5, double gAcc = 9.81, double PayloadSize = 0.25, double QuadrotorSize = 0.25); 70 | // void displayNewArrow(ros::Publisher& guide_vector_pub, payload_planner::PolyTrajOptimizer::Ptr optimizer); 71 | }; 72 | } // namespace payload_planner 73 | #endif -------------------------------------------------------------------------------- /planner/plan_manage/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /planner/plan_manage/launch/simple_run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /planner/plan_manage/launch/simulator.xml: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /planner/plan_manage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | payload_planner 4 | 0.0.0 5 | The payload_planner package 6 | 7 | 8 | 9 | 10 | Haojia Li 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | 55 | plan_env 56 | path_searching 57 | traj_opt 58 | traj_utils 59 | 60 | roscpp 61 | std_msgs 62 | 63 | roscpp 64 | std_msgs 65 | message_runtime 66 | 67 | plan_env 68 | path_searching 69 | traj_opt 70 | traj_utils 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /planner/plan_manage/src/payload_planner_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include "plan_manage/backward.hpp" 6 | 7 | // for better debugging 8 | namespace backward { 9 | backward::SignalHandling sh; 10 | } 11 | 12 | using namespace payload_planner; 13 | 14 | int main(int argc, char **argv) 15 | { 16 | 17 | ros::init(argc, argv, "payload_planner_node"); 18 | ros::NodeHandle nh("~"); 19 | 20 | ReplanFSM payload_replan; 21 | payload_replan.init(nh); 22 | 23 | ros::spin(); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /planner/traj_opt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(traj_opt) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++17 ) 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native") 7 | 8 | find_package(Eigen3 REQUIRED) 9 | find_package(PCL 1.7 REQUIRED) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | visualization_msgs 16 | plan_env 17 | cv_bridge 18 | path_searching 19 | 20 | ) 21 | 22 | catkin_package( 23 | INCLUDE_DIRS include 24 | LIBRARIES traj_opt 25 | CATKIN_DEPENDS plan_env 26 | # DEPENDS system_lib 27 | ) 28 | 29 | include_directories( 30 | SYSTEM 31 | include 32 | ${catkin_INCLUDE_DIRS} 33 | ${Eigen3_INCLUDE_DIRS} 34 | ${PCL_INCLUDE_DIRS} 35 | ${PROJECT_SOURCE_DIR} 36 | ) 37 | 38 | 39 | add_library( traj_opt 40 | src/poly_traj_optimizer.cpp 41 | ) 42 | 43 | target_link_libraries( traj_opt 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | -------------------------------------------------------------------------------- /planner/traj_opt/include/optimizer/plan_container.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _PLAN_CONTAINER_H_ 2 | #define _PLAN_CONTAINER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using std::vector; 11 | 12 | namespace payload_planner 13 | { 14 | 15 | struct GlobalTrajData 16 | { 17 | poly_traj::Trajectory<7> traj; 18 | double global_start_time; // world time 19 | double duration; 20 | 21 | /* Global traj time. 22 | The corresponding global trajectory time of the current local target. 23 | Used in local target selection process */ 24 | double glb_t_of_lc_tgt; 25 | /* Global traj time. 26 | The corresponding global trajectory time of the last local target. 27 | Used in initial-path-from-last-optimal-trajectory generation process */ 28 | double last_glb_t_of_lc_tgt; 29 | 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | }; 33 | 34 | struct LocalTrajData 35 | { 36 | poly_traj::Trajectory<7> traj; 37 | int drone_id; // A negative value indicates no received trajectories. 38 | int traj_id; 39 | double duration; 40 | double start_time; // world time 41 | double end_time; // world time 42 | Eigen::Vector3d start_pos; 43 | 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | }; 47 | 48 | class TrajContainer 49 | { 50 | public: 51 | GlobalTrajData global_traj; 52 | LocalTrajData local_traj; 53 | 54 | TrajContainer() 55 | { 56 | local_traj.traj_id = 0; 57 | } 58 | ~TrajContainer() {} 59 | 60 | void setGlobalTraj(const poly_traj::Trajectory<7> &trajectory, const double &world_time) 61 | { 62 | global_traj.traj.clear(); //Must clear in Ubuntu 18.04 63 | global_traj.traj = trajectory; 64 | global_traj.duration = trajectory.getTotalDuration(); 65 | global_traj.global_start_time = world_time; 66 | global_traj.glb_t_of_lc_tgt = world_time; 67 | global_traj.last_glb_t_of_lc_tgt = -1.0; 68 | 69 | local_traj.drone_id = -1; 70 | local_traj.duration = 0.0; 71 | local_traj.traj_id = 0; 72 | } 73 | 74 | void setLocalTraj(const poly_traj::Trajectory<7> &trajectory, const double &world_time, const int drone_id = -1) 75 | { 76 | local_traj.drone_id = drone_id; 77 | local_traj.traj_id++; 78 | local_traj.duration = trajectory.getTotalDuration(); 79 | local_traj.start_pos = trajectory.getJuncPos(0); 80 | local_traj.start_time = world_time; 81 | local_traj.traj.clear(); //Must clear in Ubuntu 18.04 82 | local_traj.traj = trajectory; 83 | } 84 | }; 85 | 86 | struct PlanParameters 87 | { 88 | /* planning algorithm parameters */ 89 | double max_vel_, max_acc_; // physical limits 90 | double polyTraj_piece_length; // distance between adjacient Spinle control points 91 | double planning_horizen_; 92 | 93 | /* processing time */ 94 | double time_search_ = 0.0; 95 | double time_optimize_ = 0.0; 96 | double time_adjust_ = 0.0; 97 | }; 98 | 99 | } // namespace payload_planner 100 | 101 | #endif -------------------------------------------------------------------------------- /planner/traj_opt/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | traj_opt 4 | 0.0.0 5 | The traj_opt package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | plan_env 56 | path_searching 57 | roscpp 58 | rospy 59 | std_msgs 60 | plan_env 61 | path_searching 62 | roscpp 63 | rospy 64 | std_msgs 65 | plan_env 66 | path_searching 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /planner/traj_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(traj_utils) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++17") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -march=native") 7 | # set(ENABLE_PRECOMPILED_HEADERS "OFF") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | geometry_msgs 13 | message_generation 14 | ) 15 | 16 | # Generate messages in the 'msg' folder 17 | add_message_files( 18 | FILES 19 | DataDisp.msg 20 | ) 21 | 22 | # Generate added messages and services with any dependencies listed here 23 | generate_messages( 24 | DEPENDENCIES 25 | std_msgs 26 | geometry_msgs 27 | ) 28 | 29 | catkin_package( 30 | # INCLUDE_DIRS include 31 | LIBRARIES traj_utils 32 | CATKIN_DEPENDS message_runtime 33 | # DEPENDS system_lib 34 | ) 35 | 36 | include_directories( 37 | SYSTEM 38 | ${catkin_INCLUDE_DIRS} 39 | # ${EIGEN3_INCLUDE_DIR} 40 | ) 41 | 42 | # link_directories(${PCL_LIBRARY_DIRS}) 43 | 44 | add_library( traj_utils 45 | src/main.cpp 46 | ) 47 | target_link_libraries( traj_utils 48 | ${catkin_LIBRARIES} 49 | ) 50 | -------------------------------------------------------------------------------- /planner/traj_utils/msg/DataDisp.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float64 a 4 | float64 b 5 | float64 c 6 | float64 d 7 | float64 e -------------------------------------------------------------------------------- /planner/traj_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | traj_utils 4 | 0.0.0 5 | The traj_utils package 6 | 7 | 8 | 9 | 10 | bzhouai 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | message_generation 55 | 56 | catkin 57 | std_msgs 58 | 59 | catkin 60 | roscpp 61 | std_msgs 62 | message_runtime 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /planner/traj_utils/src/main.cpp: -------------------------------------------------------------------------------- 1 | int empty() 2 | { 3 | /* code */ 4 | return 0; 5 | } 6 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(local_sensing_node) 2 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) 3 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 4 | #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | 8 | set(ENABLE_CUDA false) 9 | # set(ENABLE_CUDA true) 10 | 11 | if(ENABLE_CUDA) 12 | find_package(CUDA REQUIRED) 13 | SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math) 14 | set(CUDA_NVCC_FLAGS 15 | # -gencode arch=compute_20,code=sm_20; 16 | # -gencode arch=compute_20,code=sm_21; 17 | # -gencode arch=compute_30,code=sm_30; 18 | # -gencode arch=compute_35,code=sm_35; 19 | # -gencode arch=compute_50,code=sm_50; 20 | # -gencode arch=compute_52,code=sm_52; 21 | -gencode arch=compute_60,code=sm_60; 22 | # -gencode arch=compute_61,code=sm_61; 23 | # -gencode arch=compute_75,code=sm_75; 24 | ) 25 | 26 | SET(CUDA_PROPAGATE_HOST_FLAGS OFF) 27 | 28 | find_package(OpenCV REQUIRED) 29 | find_package(Eigen3 REQUIRED) 30 | find_package(Boost REQUIRED COMPONENTS system filesystem) 31 | 32 | find_package(catkin REQUIRED COMPONENTS 33 | roscpp roslib cmake_modules cv_bridge image_transport pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs dynamic_reconfigure) 34 | generate_dynamic_reconfigure_options( 35 | cfg/local_sensing_node.cfg 36 | ) 37 | catkin_package( 38 | DEPENDS OpenCV Eigen Boost 39 | CATKIN_DEPENDS roscpp roslib image_transport pcl_ros 40 | #INCLUDE_DIRS include 41 | LIBRARIES depth_render_cuda 42 | ) 43 | 44 | include_directories( 45 | SYSTEM 46 | #include 47 | ${catkin_INCLUDE_DIRS} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${Eigen_INCLUDE_DIRS} 50 | ${Boost_INCLUDE_DIRS} 51 | ) 52 | 53 | CUDA_ADD_LIBRARY( depth_render_cuda 54 | src/depth_render.cu 55 | ) 56 | 57 | add_executable( 58 | pcl_render_node 59 | src/pcl_render_node.cpp 60 | ) 61 | target_link_libraries( pcl_render_node 62 | depth_render_cuda 63 | ${OpenCV_LIBS} 64 | ${Boost_LIBRARIES} 65 | ${catkin_LIBRARIES} 66 | ) 67 | else(ENABLE_CUDA) 68 | find_package(Eigen3 REQUIRED) 69 | find_package(catkin REQUIRED COMPONENTS 70 | roscpp roslib cmake_modules pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs) 71 | 72 | catkin_package( 73 | DEPENDS Eigen 74 | CATKIN_DEPENDS roscpp roslib pcl_ros 75 | ) 76 | 77 | include_directories( 78 | SYSTEM 79 | ${catkin_INCLUDE_DIRS} 80 | ${Eigen_INCLUDE_DIRS}. 81 | ) 82 | 83 | add_executable( 84 | pcl_render_node 85 | src/pointcloud_render_node.cpp 86 | ) 87 | 88 | target_link_libraries( pcl_render_node 89 | ${catkin_LIBRARIES} 90 | ${PCL_LIBRARIES} 91 | ) 92 | endif(ENABLE_CUDA) 93 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake: -------------------------------------------------------------------------------- 1 | # James Bigler, NVIDIA Corp (nvidia.com - jbigler) 2 | # Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html 3 | # 4 | # Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. 5 | # 6 | # Copyright (c) 2007-2009 7 | # Scientific Computing and Imaging Institute, University of Utah 8 | # 9 | # This code is licensed under the MIT License. See the FindCUDA.cmake script 10 | # for the text of the license. 11 | 12 | # The MIT License 13 | # 14 | # License for the specific language governing rights and limitations under 15 | # Permission is hereby granted, free of charge, to any person obtaining a 16 | # copy of this software and associated documentation files (the "Software"), 17 | # to deal in the Software without restriction, including without limitation 18 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 19 | # and/or sell copies of the Software, and to permit persons to whom the 20 | # Software is furnished to do so, subject to the following conditions: 21 | # 22 | # The above copyright notice and this permission notice shall be included 23 | # in all copies or substantial portions of the Software. 24 | # 25 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 26 | # OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 28 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 30 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 31 | # DEALINGS IN THE SOFTWARE. 32 | # 33 | 34 | ####################################################################### 35 | # This converts a file written in makefile syntax into one that can be included 36 | # by CMake. 37 | 38 | file(READ ${input_file} depend_text) 39 | 40 | if (${depend_text} MATCHES ".+") 41 | 42 | # message("FOUND DEPENDS") 43 | 44 | # Remember, four backslashes is escaped to one backslash in the string. 45 | string(REGEX REPLACE "\\\\ " " " depend_text ${depend_text}) 46 | 47 | # This works for the nvcc -M generated dependency files. 48 | string(REGEX REPLACE "^.* : " "" depend_text ${depend_text}) 49 | string(REGEX REPLACE "[ \\\\]*\n" ";" depend_text ${depend_text}) 50 | 51 | set(dependency_list "") 52 | 53 | foreach(file ${depend_text}) 54 | 55 | string(REGEX REPLACE "^ +" "" file ${file}) 56 | 57 | if(NOT IS_DIRECTORY ${file}) 58 | # If softlinks start to matter, we should change this to REALPATH. For now we need 59 | # to flatten paths, because nvcc can generate stuff like /bin/../include instead of 60 | # just /include. 61 | get_filename_component(file_absolute "${file}" ABSOLUTE) 62 | list(APPEND dependency_list "${file_absolute}") 63 | endif(NOT IS_DIRECTORY ${file}) 64 | 65 | endforeach(file) 66 | 67 | else() 68 | # message("FOUND NO DEPENDS") 69 | endif() 70 | 71 | # Remove the duplicate entries and sort them. 72 | list(REMOVE_DUPLICATES dependency_list) 73 | list(SORT dependency_list) 74 | 75 | foreach(file ${dependency_list}) 76 | set(cuda_nvcc_depend "${cuda_nvcc_depend} \"${file}\"\n") 77 | endforeach() 78 | 79 | file(WRITE ${output_file} "# Generated by: make2cmake.cmake\nSET(CUDA_NVCC_DEPEND\n ${cuda_nvcc_depend})\n\n") 80 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3/") 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/cfg/local_sensing_node.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "local_sensing_node" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("tx", double_t, 0, "tx", 0.0, -1000.0, 1000.0) 9 | gen.add("ty", double_t, 0, "ty", 0.0, -1000.0, 1000.0) 10 | gen.add("tz", double_t, 0, "tz", 0.0, -1000.0, 1000.0) 11 | gen.add("axis_x", double_t, 0, "axis_x", 0.0, -360.0, 360.0) 12 | gen.add("axis_y", double_t, 0, "axis_y", 0.0, -360.0, 360.0) 13 | gen.add("axis_z", double_t, 0, "axis_z", 0.0, -360.0, 360.0) 14 | 15 | exit(gen.generate(PACKAGE, "local_sensing_node", "local_sensing_node")) -------------------------------------------------------------------------------- /uav_simulator/local_sensing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | local_sensing_node 4 | 0.1.0 5 | 6 | render depth from depth 7 | 8 | Matia Pizzoli 9 | Matia Pizzoli 10 | GPLv3 11 | 12 | 13 | catkin 14 | 15 | 16 | cmake_modules 17 | roscpp 18 | roslib 19 | svo_msgs 20 | cv_bridge 21 | image_transport 22 | vikit_ros 23 | pcl_ros 24 | dynamic_reconfigure 25 | quadrotor_msgs 26 | 27 | 28 | roscpp 29 | roslib 30 | svo_msgs 31 | cv_bridge 32 | image_transport 33 | vikit_ros 34 | pcl_ros 35 | dynamic_reconfigure 36 | quadrotor_msgs 37 | 38 | 39 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/params/camera.yaml: -------------------------------------------------------------------------------- 1 | cam_width: 640 2 | cam_height: 480 3 | cam_fx: 387.229248046875 4 | cam_fy: 387.229248046875 5 | cam_cx: 321.04638671875 6 | cam_cy: 243.44969177246094 7 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/AlignError.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | struct AlignError { 5 | AlignError( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, 6 | const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) 7 | { 8 | camera_q[0] = camera_pose.x(); 9 | camera_q[1] = camera_pose.y(); 10 | camera_q[2] = camera_pose.z(); 11 | camera_q[3] = camera_pose.w(); 12 | camera_t[0] = camera_trans.x(); 13 | camera_t[1] = camera_trans.y(); 14 | camera_t[2] = camera_trans.z(); 15 | 16 | velodyne_q[0] = velodyne_pose.x(); 17 | velodyne_q[1] = velodyne_pose.y(); 18 | velodyne_q[2] = velodyne_pose.z(); 19 | velodyne_q[3] = velodyne_pose.w(); 20 | velodyne_t[0] = velodyne_trans.x(); 21 | velodyne_t[1] = velodyne_trans.y(); 22 | velodyne_t[2] = velodyne_trans.z(); 23 | } 24 | 25 | // Factory to hide the construction of the CostFunction object from the client code. 26 | static ceres::CostFunction* Create( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, 27 | const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) 28 | { 29 | return (new ceres::AutoDiffCostFunction( 30 | new AlignError(camera_pose, camera_trans, velodyne_pose, velodyne_trans))); 31 | } 32 | 33 | template 34 | bool operator()(const T* const world_rotation, const T* const world_translation, 35 | const T* const v2c_rotation, const T* const v2c_translation, 36 | T* residuals) const 37 | { 38 | Eigen::Quaternion q_world = Eigen::Map< const Eigen::Quaternion >(world_rotation); 39 | Eigen::Matrix t_world = Eigen::Map< const Eigen::Matrix >(world_translation); 40 | 41 | Eigen::Quaternion q_v2c = Eigen::Map< const Eigen::Quaternion >(v2c_rotation); 42 | Eigen::Matrix t_v2c = Eigen::Map< const Eigen::Matrix >(v2c_translation); 43 | 44 | Eigen::Quaternion q_c; 45 | Eigen::Matrix t_c; 46 | q_c.x() = T(camera_q[0]); 47 | q_c.y() = T(camera_q[1]); 48 | q_c.z() = T(camera_q[2]); 49 | q_c.w() = T(camera_q[3]); 50 | t_c << T(camera_t[0]), T(camera_t[1]), T(camera_t[2]); 51 | 52 | Eigen::Quaternion q_v; 53 | Eigen::Matrix t_v; 54 | q_v.x() = T(velodyne_q[0]); 55 | q_v.y() = T(velodyne_q[1]); 56 | q_v.z() = T(velodyne_q[2]); 57 | q_v.w() = T(velodyne_q[3]); 58 | t_v << T(velodyne_t[0]), T(velodyne_t[1]), T(velodyne_t[2]); 59 | 60 | Eigen::Quaternion q_left; 61 | Eigen::Matrix t_left; 62 | 63 | q_left = q_world * q_c * q_v2c; 64 | t_left = q_world * q_c * t_v2c + q_world * t_c + t_world; 65 | 66 | Eigen::Quaternion q_diff; 67 | Eigen::Matrix t_diff; 68 | q_diff = q_left * q_v.inverse(); 69 | // t_diff = t_left - q_left * q_v.inverse() * t_v; 70 | t_diff = t_left - t_v; 71 | 72 | residuals[0] = q_diff.x(); 73 | residuals[1] = q_diff.y(); 74 | residuals[2] = q_diff.z(); 75 | residuals[3] = t_diff(0); 76 | residuals[4] = t_diff(1); 77 | residuals[5] = t_diff(2); 78 | 79 | return true; 80 | } 81 | 82 | double camera_q[4]; 83 | double camera_t[3]; 84 | double velodyne_q[4]; 85 | double velodyne_t[3]; 86 | }; 87 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/csv_convert.py: -------------------------------------------------------------------------------- 1 | import csv 2 | file_location = '/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.csv' 3 | with open('/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.txt', 'w') as txt_f: 4 | with open(file_location) as f: 5 | f_csv = csv.reader(f) 6 | headers = next(f_csv) 7 | for row in f_csv: 8 | txt_f.write('%lf\n'% (float(row[0]) / 1000000000.0) ) 9 | txt_f.write('%lf\n'% (float(row[1])) ) 10 | txt_f.write('%lf\n'% (float(row[2])) ) 11 | txt_f.write('%lf\n'% (float(row[3])) ) 12 | txt_f.write('%lf\n'% (float(row[4])) ) 13 | txt_f.write('%lf\n'% (float(row[5])) ) 14 | txt_f.write('%lf\n'% (float(row[6])) ) 15 | txt_f.write('%lf\n'% (float(row[7])) ) -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/cuda_exception.cuh: -------------------------------------------------------------------------------- 1 | // This file is part of REMODE - REgularized MOnocular Depth Estimation. 2 | // 3 | // Copyright (C) 2014 Matia Pizzoli 4 | // Robotics and Perception Group, University of Zurich, Switzerland 5 | // http://rpg.ifi.uzh.ch 6 | // 7 | // REMODE is free software: you can redistribute it and/or modify it under the 8 | // terms of the GNU General Public License as published by the Free Software 9 | // Foundation, either version 3 of the License, or any later version. 10 | // 11 | // REMODE is distributed in the hope that it will be useful, but WITHOUT ANY 12 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | // FOR A PARTICULAR PURPOSE. See the 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 | #ifndef RMD_CUDA_EXCEPTION_CUH_ 19 | #define RMD_CUDA_EXCEPTION_CUH_ 20 | 21 | #include 22 | #include 23 | 24 | 25 | struct CudaException : public std::exception 26 | { 27 | CudaException(const std::string& what, cudaError err) 28 | : what_(what), err_(err) {} 29 | virtual ~CudaException() throw() {} 30 | virtual const char* what() const throw() 31 | { 32 | std::stringstream description; 33 | description << "CudaException: " << what_ << std::endl; 34 | if(err_ != cudaSuccess) 35 | { 36 | description << "cudaError code: " << cudaGetErrorString(err_); 37 | description << " (" << err_ << ")" << std::endl; 38 | } 39 | return description.str().c_str(); 40 | } 41 | std::string what_; 42 | cudaError err_; 43 | }; 44 | 45 | #endif /* RMD_CUDA_EXCEPTION_CUH_ */ 46 | -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/depth_render.cuh: -------------------------------------------------------------------------------- 1 | #ifndef DEPTH_RENDER_CUH 2 | #define DEPTH_RENDER_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | //#include 11 | 12 | #include "device_image.cuh" 13 | #include "cuda_exception.cuh" 14 | #include "helper_math.h" 15 | 16 | using namespace std; 17 | //using namespace Eigen; 18 | 19 | struct Parameter 20 | { 21 | int point_number; 22 | float fx, fy, cx, cy; 23 | int width, height; 24 | float r[3][3]; 25 | float t[3]; 26 | }; 27 | 28 | class DepthRender 29 | { 30 | public: 31 | DepthRender(); 32 | void set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height); 33 | ~DepthRender(); 34 | void set_data(vector &cloud_data); 35 | //void render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr); 36 | //void render_pose( Matrix4d &transformation, int *host_ptr); 37 | void render_pose( double * transformation, int *host_ptr); 38 | 39 | private: 40 | int cloud_size; 41 | 42 | //data 43 | float3 *host_cloud_ptr; 44 | float3 *dev_cloud_ptr; 45 | bool has_devptr; 46 | 47 | //camera 48 | Parameter parameter; 49 | Parameter* parameter_devptr; 50 | }; 51 | 52 | #endif -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/empty.cpp: -------------------------------------------------------------------------------- 1 | #include "empty.h" -------------------------------------------------------------------------------- /uav_simulator/local_sensing/src/empty.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotics-STAR-Lab/AutoTrans/c8e50fa92ceab8244cba9f6c3a552ab157082363/uav_simulator/local_sensing/src/empty.h -------------------------------------------------------------------------------- /uav_simulator/map_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(map_generator) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++14 ) 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy 11 | std_msgs 12 | geometry_msgs 13 | pcl_conversions 14 | ) 15 | find_package(PCL REQUIRED) 16 | find_package(Eigen3 REQUIRED) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES ${PROJECT_NAME} 21 | CATKIN_DEPENDS 22 | ) 23 | 24 | include_directories( 25 | include 26 | SYSTEM 27 | ${catkin_INCLUDE_DIRS} 28 | ${EIGEN3_INCLUDE_DIR} 29 | ${PCL_INCLUDE_DIRS} 30 | ${PROJECT_SOURCE_DIR}/include 31 | ) 32 | 33 | add_executable (random_forest src/random_forest_sensing.cpp ) 34 | target_link_libraries(random_forest 35 | ${catkin_LIBRARIES} 36 | ${PCL_LIBRARIES}) 37 | 38 | add_executable (map_generator src/map_generator.cpp ) 39 | target_link_libraries(map_generator 40 | ${catkin_LIBRARIES} 41 | ${PCL_LIBRARIES}) 42 | -------------------------------------------------------------------------------- /uav_simulator/map_generator/include/map_generator/map_generator.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | // #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace map_generator 19 | { 20 | class MapGenerator 21 | { 22 | public: 23 | void init(ros::NodeHandle &nh); 24 | void pubPoints(void); 25 | void generateMap(void); 26 | 27 | typedef std::unique_ptr Ptr; 28 | 29 | private: 30 | ros::NodeHandle nh_; 31 | pcl::KdTreeFLANN kdtreeLocalMap_; 32 | 33 | ros::Publisher map_pub_; 34 | 35 | struct GeneratorParameters 36 | { 37 | int seed_; 38 | int obs_num_, circle_num_; 39 | double x_size_, y_size_, z_size_; 40 | double resolution_; 41 | 42 | double x_l_, x_h_, y_l_, y_h_, z_l_, z_h_; 43 | double radius_l_, radius_h_; 44 | double w_l_, w_h_, h_l_, h_h_; 45 | 46 | double z_limit_, sensing_range_, pub_rate_; 47 | double min_dist_; 48 | int fix_obs_type_; 49 | 50 | bool map_ok_ = false; 51 | 52 | double theta_; 53 | } generator_params_; 54 | 55 | sensor_msgs::PointCloud2 globalMap_pcd_; 56 | pcl::PointCloud cloud_map_; 57 | 58 | sensor_msgs::PointCloud2 localMap_pcd_; 59 | pcl::PointCloud clicked_cloud__; 60 | 61 | void generateWall(std::vector ¶ms, 62 | pcl::PointCloud &cloud_map); 63 | void generateCylinder(std::vector ¶ms, 64 | pcl::PointCloud &cloud_map); 65 | void generateRing(std::vector ¶ms, 66 | pcl::PointCloud &cloud_map); 67 | }; 68 | } 69 | -------------------------------------------------------------------------------- /uav_simulator/map_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | map_generator 4 | 0.0.0 5 | The map_generator package 6 | 7 | 8 | 9 | 10 | bzhouai 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/README.md: -------------------------------------------------------------------------------- 1 | # mockamap 2 | a simple map generator based on ROS 3 | 4 | demo cases: 5 | 6 | * perlin3d map 7 | 8 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/perlin3d.png) 9 | 10 | * post2d map 11 | 12 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/post2d.png) 13 | 14 | @misc{mockamap, 15 | author = "William.Wu", 16 | title = "mockamap", 17 | howpublished = "\url{https://github.com/HKUST-Aerial-Robotics/mockamap }", 18 | } 19 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/include/maps.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAPS_HPP 2 | #define MAPS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mocka { 11 | 12 | class Maps { 13 | public: 14 | typedef struct BasicInfo { 15 | ros::NodeHandle *nh_private; 16 | int sizeX; 17 | int sizeY; 18 | int sizeZ; 19 | int seed; 20 | double scale; 21 | sensor_msgs::PointCloud2 *output; 22 | pcl::PointCloud *cloud; 23 | } BasicInfo; 24 | 25 | BasicInfo getInfo() const; 26 | void setInfo(const BasicInfo &value); 27 | 28 | public: 29 | Maps(); 30 | 31 | public: 32 | void generate(int type); 33 | 34 | private: 35 | BasicInfo info; 36 | 37 | private: 38 | void pcl2ros(); 39 | 40 | void perlin3D(); 41 | void maze2D(); 42 | void randomMapGenerate(); 43 | void Maze3DGen(); 44 | void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze); 45 | void recursizeDivisionMaze(Eigen::MatrixXi &maze); 46 | void optimizeMap(); 47 | }; 48 | 49 | class MazePoint { 50 | private: 51 | pcl::PointXYZ point; 52 | double dist1; 53 | double dist2; 54 | int point1; 55 | int point2; 56 | bool isdoor; 57 | 58 | public: 59 | pcl::PointXYZ getPoint(); 60 | int getPoint1(); 61 | int getPoint2(); 62 | double getDist1(); 63 | double getDist2(); 64 | void setPoint(pcl::PointXYZ p); 65 | void setPoint1(int p); 66 | void setPoint2(int p); 67 | void setDist1(double set); 68 | void setDist2(double set); 69 | }; 70 | 71 | } // namespace mocka 72 | 73 | #endif // MAPS_HPP 74 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/include/perlinnoise.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PERLINNOISE_HPP 2 | #define PERLINNOISE_HPP 3 | 4 | #include 5 | 6 | // THIS CLASS IS A TRANSLATION TO C++14 FROM THE REFERENCE 7 | // JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see 8 | // http://mrl.nyu.edu/~perlin/noise/) 9 | // THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN 10 | 11 | // I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT 12 | // PRESENT IN THE ORIGINAL IMPLEMENTATION) 13 | 14 | class PerlinNoise 15 | { 16 | // The permutation vector 17 | std::vector p; 18 | 19 | public: 20 | // Initialize with the reference values for the permutation vector 21 | PerlinNoise(); 22 | // Generate a new permutation vector based on the value of seed 23 | PerlinNoise(unsigned int seed); 24 | // Get a noise value, for 2D images z can have any value 25 | double noise(double x, double y, double z); 26 | 27 | private: 28 | double fade(double t); 29 | double lerp(double t, double a, double b); 30 | double grad(int hash, double x, double y, double z); 31 | }; 32 | 33 | #endif // PERLINNOISE_HPP 34 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/launch/maze2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/launch/maze3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/launch/mockamap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 24 | 25 | 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 | 56 | 57 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/launch/perlin3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/launch/post2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mockamap 4 | 0.1.0 5 | The mockamap package 6 | 7 | 8 | 9 | 10 | William.Wu 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | William.Wu 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | roscpp 45 | pcl_ros 46 | pcl_conversions 47 | roscpp 48 | pcl_ros 49 | pcl_conversions 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/src/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Mozilla 3 | Standard: Cpp03 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: true 6 | AlignConsecutiveDeclarations: true 7 | AllowShortFunctionsOnASingleLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | BreakBeforeBraces: Allman 11 | ... 12 | 13 | -------------------------------------------------------------------------------- /uav_simulator/mockamap/src/mockamap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "maps.hpp" 13 | 14 | void 15 | optimizeMap(mocka::Maps::BasicInfo& in) 16 | { 17 | std::vector* temp = new std::vector; 18 | 19 | pcl::KdTreeFLANN kdtree; 20 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 21 | 22 | cloud->width = in.cloud->width; 23 | cloud->height = in.cloud->height; 24 | cloud->points.resize(cloud->width * cloud->height); 25 | 26 | for (int i = 0; i < cloud->width; i++) 27 | { 28 | cloud->points[i].x = in.cloud->points[i].x; 29 | cloud->points[i].y = in.cloud->points[i].y; 30 | cloud->points[i].z = in.cloud->points[i].z; 31 | } 32 | 33 | kdtree.setInputCloud(cloud); 34 | double radius = 1.75 / in.scale; // 1.75 is the rounded up value of sqrt(3) 35 | 36 | for (int i = 0; i < cloud->width; i++) 37 | { 38 | std::vector pointIdxRadiusSearch; 39 | std::vector pointRadiusSquaredDistance; 40 | 41 | if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, 42 | pointRadiusSquaredDistance) >= 27) 43 | { 44 | temp->push_back(i); 45 | } 46 | } 47 | for (int i = temp->size() - 1; i >= 0; i--) 48 | { 49 | in.cloud->points.erase(in.cloud->points.begin() + 50 | temp->at(i)); // erasing the enclosed points 51 | } 52 | in.cloud->width -= temp->size(); 53 | 54 | pcl::toROSMsg(*in.cloud, *in.output); 55 | in.output->header.frame_id = "world"; 56 | ROS_INFO("finish: number of points after optimization %d", in.cloud->width); 57 | delete temp; 58 | return; 59 | } 60 | 61 | int 62 | main(int argc, char** argv) 63 | { 64 | ros::init(argc, argv, "mockamap"); 65 | ros::NodeHandle nh; 66 | ros::NodeHandle nh_private("~"); 67 | 68 | ros::Publisher pcl_pub = 69 | nh.advertise("mock_map", 1); 70 | pcl::PointCloud cloud; 71 | sensor_msgs::PointCloud2 output; 72 | // Fill in the cloud data 73 | 74 | int seed; 75 | 76 | int sizeX; 77 | int sizeY; 78 | int sizeZ; 79 | 80 | double scale; 81 | double update_freq; 82 | 83 | int type; 84 | 85 | nh_private.param("seed", seed, 4546); 86 | nh_private.param("update_freq", update_freq, 1.0); 87 | nh_private.param("resolution", scale, 0.38); 88 | nh_private.param("x_length", sizeX, 100); 89 | nh_private.param("y_length", sizeY, 100); 90 | nh_private.param("z_length", sizeZ, 10); 91 | 92 | nh_private.param("type", type, 1); 93 | 94 | scale = 1 / scale; 95 | sizeX = sizeX * scale; 96 | sizeY = sizeY * scale; 97 | sizeZ = sizeZ * scale; 98 | 99 | mocka::Maps::BasicInfo info; 100 | info.nh_private = &nh_private; 101 | info.sizeX = sizeX; 102 | info.sizeY = sizeY; 103 | info.sizeZ = sizeZ; 104 | info.seed = seed; 105 | info.scale = scale; 106 | info.output = &output; 107 | info.cloud = &cloud; 108 | 109 | mocka::Maps map; 110 | map.setInfo(info); 111 | map.generate(type); 112 | 113 | // optimizeMap(info); 114 | 115 | //! @note publish loop 116 | ros::Rate loop_rate(update_freq); 117 | while (ros::ok()) 118 | { 119 | pcl_pub.publish(output); 120 | ros::spinOnce(); 121 | loop_rate.sleep(); 122 | } 123 | return 0; 124 | } 125 | -------------------------------------------------------------------------------- /uav_simulator/so3_quadrotor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(so3_quadrotor) 3 | 4 | add_compile_options(-std=c++14) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") 7 | 8 | find_package(Eigen3 REQUIRED) 9 | find_package(catkin REQUIRED COMPONENTS 10 | quadrotor_msgs 11 | roscpp 12 | nodelet 13 | tf 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} 18 | CATKIN_DEPENDS quadrotor_msgs roscpp nodelet tf 19 | DEPENDS Eigen3 REQUIRED 20 | ) 21 | 22 | include_directories( 23 | include 24 | ${EIGEN3_INCLUDE_DIR} 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | add_library(${PROJECT_NAME}_nodelet 29 | src/${PROJECT_NAME}_nodelet.cpp 30 | ) 31 | 32 | target_link_libraries(${PROJECT_NAME}_nodelet 33 | ${catkin_LIBRARIES} 34 | ) -------------------------------------------------------------------------------- /uav_simulator/so3_quadrotor/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /uav_simulator/so3_quadrotor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | so3_quadrotor 4 | 0.0.0 5 | The quadrotor with payload simulation package 6 | 7 | Haojia Li 8 | 9 | GPLv3 10 | 11 | catkin 12 | quadrotor_msgs 13 | roscpp 14 | nodelet 15 | tf 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /uav_simulator/uav_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(uav_simulator) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") 7 | 8 | find_package(catkin REQUIRED COMPONENTS) 9 | 10 | catkin_package() 11 | 12 | include_directories( 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | -------------------------------------------------------------------------------- /uav_simulator/uav_simulator/config/local_sensing.yaml: -------------------------------------------------------------------------------- 1 | camera_rate: 30.0 2 | camera_range: 5.0 3 | 4 | cam_width: 640 5 | cam_height: 480 6 | cam_fx: 387.229248046875 7 | cam_fy: 387.229248046875 8 | cam_cx: 321.04638671875 9 | cam_cy: 243.44969177246094 10 | 11 | depth_scaling_factor: 1000.0 -------------------------------------------------------------------------------- /uav_simulator/uav_simulator/config/mockamap.yaml: -------------------------------------------------------------------------------- 1 | seed: 510 2 | update_freq: 1.0 3 | resolution: 0.1 4 | 5 | # map size unit meter 6 | x_length: 10 7 | y_length: 10 8 | z_length: 8 9 | 10 | # 1: perlin noise 3D 11 | type: 5 12 | 13 | # large value will be complex 14 | # typical 0.0 ~ 0.5 15 | # complexity: 0.02 16 | # infill persentage 17 | # typical: 0.4 ~ 0.0 18 | # fill: 0.3 19 | # large value will have more detail 20 | # fractal: 1 21 | # for fractal attenuation 22 | # typical: 0.0 ~ 0.5 23 | # attenuation: 0.1 24 | 25 | # # 2: perlin box random map 26 | # type: 2 27 | # width_min: 0.6 28 | # width_max: 1.5 29 | # obstacle_number: 50 30 | 31 | # # 3: 2d maze still developing 32 | # type: 3 33 | # road_width: 0.5 34 | # add_wall_x: 0 35 | # add_wall_y: 1 36 | # maze_type: 1 37 | 38 | # # 4: maze 3d 39 | numNodes: 40 40 | connectivity: 0.1 41 | nodeRad: 1 42 | roadRad: 10 43 | 44 | # # 5: random gates 45 | numGates: 10 46 | gateSize: 2 47 | maxAngle: 10 -------------------------------------------------------------------------------- /uav_simulator/uav_simulator/config/so3_quadrotor.yaml: -------------------------------------------------------------------------------- 1 | # init_x: -3 2 | # init_y: 0 3 | # init_z: -0.3 4 | # init_quadx: -3 5 | # init_quady: 0 6 | # init_quadz: -0.5 7 | init_yaw: 0.0 8 | 9 | noise_payload: 0.000 10 | noise_uav: 0.00 11 | noise_vel_payload: 0.0 12 | noise_vel_uav: 0.0 13 | noise_acc_uav: 0.0 14 | noise_acc_payload: 0.0 15 | noise_rpm: 0.0 16 | simulation_rate: 1000 17 | odom_rate: 400 18 | g: 9.795 19 | mass: 1.150 20 | Ixx: 2.64e-3 21 | Iyy: 2.64e-3 22 | Izz: 4.96e-3 23 | kf: 8.98132e-9 24 | prop_radius: 0.062 25 | arm_length: 0.26 26 | motor_time_constant: 0.03333 27 | max_rpm: 45000.0 28 | min_rpm: 1200.0 29 | 30 | payload_size: 0.1 31 | mass_load: 0.283 32 | l_length: 0.66844507 33 | 34 | kOmega: [3.0, 3.0, 3.0] 35 | kdOmega: [15.0, 15.0, 15.0] -------------------------------------------------------------------------------- /uav_simulator/uav_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uav_simulator 4 | 0.0.0 5 | The uav_simulator package 6 | 7 | ji 8 | 9 | GPLv3 10 | 11 | catkin 12 | 13 | 14 | --------------------------------------------------------------------------------