├── .github ├── config.yml ├── stale.yml └── workflows │ ├── melodic.yml │ └── noetic.yml ├── .gitingnore ├── BebopS ├── CHANGELOG.rst ├── CITATION.cff ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── config ├── defaults.yaml └── wind_turbine.yaml ├── data └── bebop2_camera_calib.yaml ├── include ├── bebop_msgs │ └── default_topics.h ├── bebop_simulator │ ├── Matrix3x3.h │ ├── MinMax.h │ ├── QuadWord.h │ ├── Quaternion.h │ ├── Scalar.h │ ├── Vector3.h │ ├── common.h │ ├── extendedKalmanFilter.h │ ├── filter_parameters.h │ ├── parameters.h │ ├── parameters_ros.h │ ├── position_controller.h │ ├── position_controller_with_bebop.h │ ├── stabilizer_types.h │ ├── transform_datatypes.h │ ├── waypoint_filter.h │ └── waypointfilter_parameters.h └── bebop_simulator_msgs │ └── default_topics.h ├── launch ├── bebop_without_controller.launch ├── gazebo_bebop.launch ├── multiple_bebop_without_controller.launch ├── multiple_task2_world.launch ├── spawn_bebop.launch ├── spawn_wind_turbine.launch ├── task1_world.launch ├── task1_world_with_bebop.launch ├── task2_world.launch ├── task2_world_with_bebop.launch ├── task3_world.launch └── wind_turbine_control.launch ├── meshes ├── ardrone_indoor.dae ├── bebop.dae ├── propeller_ccw.dae ├── propeller_cw.dae ├── wind_turbine_base.dae └── wind_turbine_rotor.dae ├── models ├── grass_plane │ ├── CMakeLists.txt │ ├── materials │ │ ├── CMakeLists.txt │ │ ├── scripts │ │ │ ├── CMakeLists.txt │ │ │ └── grass.material │ │ └── textures │ │ │ ├── CMakeLists.txt │ │ │ └── grass_dry.png │ ├── model.config │ └── model.sdf ├── ground_plane_residential │ ├── model-1_2.sdf │ ├── model-1_3.sdf │ ├── model-1_4.sdf │ ├── model.config │ └── model.sdf ├── occupancy_grid │ ├── model.config │ └── occupancy_grid.sdf ├── vrc_heightmap_1 │ ├── CMakeLists.txt │ ├── materials │ │ ├── CMakeLists.txt │ │ └── textures │ │ │ ├── CMakeLists.txt │ │ │ └── heightmap.png │ ├── model.config │ └── model.sdf └── yosemite │ ├── materials │ └── textures │ │ ├── texture_forest.png │ │ ├── texture_mountains.png │ │ └── yosemite.png │ ├── model.config │ └── yosemite.sdf ├── package.xml ├── resource ├── EKF_matrix.yaml ├── bebop.yaml ├── controller_bebop.yaml ├── waypoint_filter.yaml ├── waypoints.txt ├── waypoints0.txt ├── waypoints1.txt ├── waypoints2.txt └── waypoints3.txt ├── rviz └── default.rviz ├── src ├── library │ ├── extendedKalmanFilter.cpp │ ├── position_controller.cpp │ ├── position_controller_with_bebop.cpp │ └── waypoint_filter.cpp └── nodes │ ├── hovering_example.cpp │ ├── position_controller_node.cpp │ ├── position_controller_node.h │ ├── position_controller_with_bebop_node.cpp │ ├── position_controller_with_bebop_node.h │ ├── quaternion_to_rpy.cpp │ └── waypoint_example.cpp ├── urdf ├── bebop.urdf.xacro ├── bebop_base.urdf.xacro ├── multirotor_base.urdf.xacro ├── wind_turbine.base.urdf.xacro └── wind_turbine.gazebo.xacro └── worlds └── basic.world /.github/config.yml: -------------------------------------------------------------------------------- 1 | # Configuration for new-issue-welcome - https://github.com/behaviorbot/new-issue-welcome 2 | 3 | # Comment to be posted to on first time issues 4 | newIssueWelcomeComment: > 5 | Thanks for opening your first issue here! Make sure that no other issues on the same topic have already been opened! 6 | # Configuration for new-pr-welcome - https://github.com/behaviorbot/new-pr-welcome 7 | 8 | # Comment to be posted to on PRs from first time contributors in your repository 9 | newPRWelcomeComment: > 10 | Thanks for opening this pull request! It will be checked as soon as possible. Please stay tuned on the page. 11 | # Configuration for first-pr-merge - https://github.com/behaviorbot/first-pr-merge 12 | 13 | # Comment to be posted to on pull requests merged by a first time user 14 | firstPRMergeComment: > 15 | Congrats on merging your first pull request! We here at behaviorbot are proud of you! Thanks to you, the package increases and is now bigger than before. 16 | # It is recommend to include as many gifs and emojis as possible 17 | 18 | # *Required* Comment to reply with 19 | requestInfoReplyComment: > 20 | We would appreciate it if you could provide us with more info about this issue :grin: 21 | -------------------------------------------------------------------------------- /.github/stale.yml: -------------------------------------------------------------------------------- 1 | # Number of days of inactivity before an issue becomes stale 2 | daysUntilStale: 120 3 | # Number of days of inactivity before a stale issue is closed 4 | daysUntilClose: 7 5 | # Issues with these labels will never be considered stale 6 | exemptLabels: 7 | - pinned 8 | - security 9 | # Label to use when marking an issue as stale 10 | staleLabel: wontfix 11 | # Comment to post when marking an issue as stale. Set to `false` to disable 12 | markComment: > 13 | This issue has been automatically marked as stale because it has not had 14 | recent activity. It will be closed if no further activity occurs. Thank you 15 | for your contributions. 16 | # Comment to post when closing a stale issue. Set to `false` to disable 17 | closeComment: false 18 | -------------------------------------------------------------------------------- /.github/workflows/melodic.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: 'Melodic' 4 | 5 | # Controls when the action will run. Triggers the workflow on push or pull request 6 | # events but only for the master branch 7 | on: 8 | push: 9 | paths-ignore: 10 | - '**/README.md' 11 | - '**.yaml' 12 | - '**.yml' 13 | - '**.launch' 14 | branches: 15 | - 'dev/ros-melodic' 16 | pull_request: 17 | branches: 18 | - 'master' 19 | - 'dev/bebopInterface' 20 | - 'dev/gazebo9' 21 | - 'dev/mitProject2019' 22 | - 'sphinx' 23 | - 'dev/ros-melodic' 24 | - 'feature/ros-kinetic-gazebo9-pkgs' 25 | - 'feature/spline' 26 | ########################################################################################### 27 | ## MANUALLY TRIGGER BUILD 28 | ########################################################################################### 29 | workflow_dispatch: 30 | 31 | jobs: 32 | ########################################################################################### 33 | ## GITHUB ACTIONS - CANCELS PREVIOUS RUNS 34 | ########################################################################################### 35 | cancel: 36 | 37 | name: Cancel Previous Runs 38 | runs-on: ubuntu-18.04 39 | steps: 40 | - name: Cancel Previous Runs 41 | uses: styfle/cancel-workflow-action@0.8.0 42 | with: 43 | access_token: ${{ github.token }} 44 | 45 | ########################################################################################## 46 | ## GITHUB ACTIONS - ROS MELODIC WITH GAZEBO 9 47 | ########################################################################################### 48 | build: 49 | name: Build on Ubuntu 18.04 with ROS Melodic 50 | runs-on: ubuntu-18.04 51 | steps: 52 | - uses: actions/checkout@v2 53 | - name: Install ROS 54 | env: 55 | ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 56 | CI_SOURCE_PATH: $(pwd) 57 | ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall 58 | CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options 59 | ROS_PARALLEL_JOBS: '-j8 -l6' 60 | # Set the python path manually to include /usr/-/python2.7/dist-packages 61 | # as this is where apt-get installs python packages. 62 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 63 | ROS_DISTRO: melodic 64 | run: | 65 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 66 | wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 67 | sudo apt-get update -qq 68 | sudo apt-get install dpkg 69 | sudo rm /var/lib/dpkg/lock 70 | sudo dpkg --configure -a 71 | sudo apt install -y python-catkin-tools python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-desktop-full 72 | sudo apt-get install -y ros-$ROS_DISTRO-joy python-rosinstall python-rosinstall-generator python-wstool build-essential 73 | sudo apt install -y ros-$ROS_DISTRO-octomap-ros ros-$ROS_DISTRO-mavlink python-rosinstall build-essential protobuf-compiler 74 | sudo apt install -y libgoogle-glog-dev python-rosinstall-generator ros-$ROS_DISTRO-pointcloud-to-laserscan 75 | sudo apt install -y python-rosdep ros-$ROS_DISTRO-octomap ros-$ROS_DISTRO-octomap-mapping 76 | sudo apt-get install -y ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-control-toolbox 77 | source /opt/ros/$ROS_DISTRO/setup.bash 78 | sudo rosdep init 79 | rosdep update --include-eol-distros 80 | rosdep install octomap_mapping 81 | rosmake octomap_mapping 82 | ##################################################################################### 83 | - name: Installation completed! Started the building process.. 84 | env: 85 | ROS_DISTRO: melodic 86 | run: | 87 | source /opt/ros/$ROS_DISTRO/setup.bash 88 | mkdir -p ~/catkin_ws/src 89 | catkin_init_workspace 90 | cd ~/catkin_ws 91 | catkin init 92 | cd ~/catkin_ws/src 93 | git clone -b med18_gazebo9 https://github.com/gsilano/rotors_simulator.git 94 | git clone -b med18_gazebo9 https://github.com/gsilano/mav_comm.git 95 | git clone https://github.com/gsilano/bebop_autonomy.git 96 | git clone -b dev/gazebo9 https://github.com/gsilano/BebopS.git 97 | rosdep update 98 | cd ~/catkin_ws 99 | rosdep install --from-paths src -i 100 | rosdep update 101 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False 102 | catkin build 103 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 104 | source ~/.bashrc 105 | -------------------------------------------------------------------------------- /.github/workflows/noetic.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: 'Noetic' 4 | 5 | # Controls when the action will run. Triggers the workflow on push or pull request 6 | # events but only for the master branch 7 | on: 8 | push: 9 | paths-ignore: 10 | - '**/README.md' 11 | - '**.yaml' 12 | - '**.yml' 13 | - '**.launch' 14 | branches: 15 | - 'dev/ros-noetic' 16 | pull_request: 17 | branches: 18 | - 'master' 19 | - 'dev/bebopInterface' 20 | - 'dev/gazebo9' 21 | - 'dev/mitProject2019' 22 | - 'sphinx' 23 | - 'feature/ros-kinetic-gazebo9-pkgs' 24 | - 'feature/spline' 25 | - 'dev/ros-noetic' 26 | ########################################################################################### 27 | ## MANUALLY TRIGGER BUILD 28 | ########################################################################################### 29 | workflow_dispatch: 30 | 31 | jobs: 32 | ########################################################################################### 33 | ## GITHUB ACTIONS - CANCELS PREVIOUS RUNS 34 | ########################################################################################### 35 | cancel: 36 | 37 | name: Cancel Previous Runs 38 | runs-on: ubuntu-20.04 39 | steps: 40 | - name: Cancel Previous Runs 41 | uses: styfle/cancel-workflow-action@0.8.0 42 | with: 43 | access_token: ${{ github.token }} 44 | 45 | ########################################################################################## 46 | ## GITHUB ACTIONS - ROS NOETIC WITH GAZEBO 11 47 | ########################################################################################### 48 | build: 49 | name: Build on Ubuntu 20.04 with ROS Noetic 50 | runs-on: ubuntu-20.04 51 | steps: 52 | - uses: actions/checkout@v2 53 | - name: Install ROS 54 | env: 55 | ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 56 | CI_SOURCE_PATH: $(pwd) 57 | ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall 58 | CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options 59 | ROS_PARALLEL_JOBS: '-j8 -l6' 60 | # Set the python path manually to include /usr/-/python2.7/dist-packages 61 | # as this is where apt-get installs python packages. 62 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 63 | ROS_DISTRO: noetic 64 | run: | 65 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 66 | wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 67 | sudo apt-get update -qq 68 | sudo apt-get install dpkg 69 | sudo rm /var/lib/dpkg/lock 70 | sudo dpkg --configure -a 71 | sudo apt install -y ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-octomap-ros ros-$ROS_DISTRO-mavlink 72 | sudo apt install -y ros-$ROS_DISTRO-octomap-mapping ros-$ROS_DISTRO-control-toolbox build-essential 73 | sudo apt install -y python3-vcstool python3-catkin-tools protobuf-compiler libgoogle-glog-dev 74 | sudo apt install -y python3-catkin-pkg python3-rosdep python3-wstool ros-$ROS_DISTRO-ros 75 | sudo apt install -y python3-rosinstall python3-rosinstall-generator 76 | sudo apt install -y ros-$ROS_DISTRO-pointcloud-to-laserscan 77 | sudo apt install -y python3-rosdep ros-$ROS_DISTRO-octomap ros-$ROS_DISTRO-octomap-mapping 78 | source /opt/ros/$ROS_DISTRO/setup.bash 79 | sudo rosdep init 80 | rosdep update --include-eol-distros 81 | rosdep install octomap_mapping 82 | rosmake octomap_mapping 83 | ##################################################################################### 84 | - name: Installation completed! Started the building process.. 85 | env: 86 | ROS_DISTRO: noetic 87 | run: | 88 | source /opt/ros/$ROS_DISTRO/setup.bash 89 | mkdir -p ~/catkin_ws/src 90 | catkin_init_workspace 91 | cd ~/catkin_ws/ 92 | catkin init 93 | cd ~/catkin_ws/src 94 | git clone -b dev/ros-noetic https://github.com/gsilano/rotors_simulator.git 95 | git clone -b med18_gazebo9 https://github.com/gsilano/mav_comm.git 96 | git clone -b dev/ros-noetic https://github.com/gsilano/BebopS.git 97 | rosdep update 98 | cd ~/catkin_ws 99 | rosdep install --from-paths src -i 100 | rosdep update 101 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False 102 | catkin build 103 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 104 | source ~/.bashrc 105 | -------------------------------------------------------------------------------- /.gitingnore: -------------------------------------------------------------------------------- 1 | # Visual Studio 2 | .vscode 3 | 4 | .DS_Store 5 | .cproject 6 | .project 7 | .settings 8 | *.pyc 9 | *~ 10 | */models/iris/*.sdf 11 | *.user 12 | 13 | # Prerequisites 14 | *.d 15 | 16 | # Compiled Object files 17 | *.slo 18 | *.lo 19 | *.o 20 | *.obj 21 | 22 | # Precompiled Headers 23 | *.gch 24 | *.pch 25 | 26 | # Compiled Dynamic libraries 27 | *.so 28 | *.dylib 29 | *.dll 30 | 31 | # Fortran module files 32 | *.mod 33 | *.smod 34 | 35 | # Compiled Static libraries 36 | *.lai 37 | *.la 38 | *.a 39 | *.lib 40 | 41 | # Executables 42 | *.exe 43 | *.out 44 | *.app 45 | -------------------------------------------------------------------------------- /BebopS: -------------------------------------------------------------------------------- 1 | BebopS -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package BebopS 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.2 (2020-08-26) 6 | ------------------ 7 | * Add multi-robot example #29 8 | * Add a default RVIZ configuration file #28 9 | * Contributors: tomlogan501, Collin-Thornton 10 | 11 | 1.0.1 (2019-12-27) 12 | ------------------ 13 | * Updated the README.md file with instructions for Ubuntu 16.04, ROS Kinetic Kame and Gazebo 9 (master) 14 | * Contributors: Giuseppe Silano 15 | 16 | 0.2.2 (2019-12-26) 17 | ------------------ 18 | * Inserted the default RotorS IMU (ADIS16448 IMU) in the list of the available sensors when running the simulation 19 | * Disabled shadows in Gazebo 9 and Gazebo 7 20 | * Contributors: Giuseppe Silano 21 | 22 | 0.2.1 (2019-04-04) 23 | ------------------ 24 | * The namespace was modified considering the ROS rules (only lowercase letters) 25 | * Improved the code quality and typos in the README.md file 26 | * Contributors: Giuseppe Silano 27 | 28 | 0.2.0 (2019-01-31) 29 | ------------------ 30 | * The package has been renamed considering the aim of the repository. Therefore, the CMake and include files have been modified based on these changes. 31 | * The files contained into the med_uav_description folder have been moved to the BebopS repository 32 | * The controller architecture, which follows the position_controller.cpp library, has been modified as described in the ECC'19 paper. For more information on how it works, please take a look at the publications section into the Wiki section. 33 | * When simulating the Parrot Bebop 2 that follows a path along the scenario (it is described through a sequence of waypoints), it is possible to decide which sensor will be simulated or not and also decide whether the real-time plots should be shown. 34 | * Contributors: Giuseppe Silano, Luigi Iannelli 35 | 36 | 0.1.6 (2018-06-18) 37 | ------------------ 38 | * A plot relating to the attitude of the aircraft has been included in the launch files of the task1 and task2 as described in the 26th MED18 Aerial Robotics and Perception Challenge. 39 | * The hovering and waypoints publishers have been inserted as nodes in the repository. 40 | * Task2 has been added. Such task models the drone trajectory tracking (it is modeled by using a sequence of waypoints). The same task was integrated also with the Parrot Bebop2 control algorithm interface. 41 | * A waypoint filter has been added to avoid system instabilities. 42 | * Bug fixing. In particular, it has been fixed the bag into the Extended Kalman Filter: the linear velocity in ABC reference system was used to control the UAV. 43 | * Typos fixing 44 | * Contributors: Giuseppe Silano, Pasquale Opppido, Luigi Iannelli 45 | 46 | 0.1.5 (2018-06-13) 47 | ------------------ 48 | * The controller node has been developed that can receive data from and send commands to the Parrot Bebop 2. This node is integrated with the status estimator, i.e., the Extended Kalman filter and the position control algorithm. The filters only take into account the noisy position and the linear speed. 49 | *The wind gusts have been integrated into the Gazebo 3D environment according to the task1 of the 26th MED aerial robotics and perception challenge. 50 | * Contributors: Pasquale Oppido, Giuseppe Silano, Luigi Iannelli 51 | 52 | 0.1.4 (2018-06-11) 53 | ------------------ 54 | * Added the Kalman filter 55 | * Added plots in the launch file to monitor the position and linear velocity errors between the Kalman filter output and the odometry ground truth values. In order to develop such functionality, the teamsannio messages have been created. 56 | * A yaml file needed to set up the Kalman filter has been created. Such file contains the tuning matrix of the filter and the standard deviations that characterize the odometry virtual sensor. 57 | * The Kalman filter works with the noisy attitude (standard deviation 0.017). This function considers the nonideality of the attitude filter onboard the quadrotor. 58 | * Contributors: Pasquale Oppido, Giuseppe Silano, Luigi Iannelli 59 | 60 | 0.1.3 (2018-05-25) 61 | ----------- 62 | * Added the data storage section in the position controller file. This section allows to storage, in suitable csv files, the aircraft and controller states. 63 | * Bug fixing 64 | * The sim time has been added in the launch file. Now the position controller node uses the simulation and not wall clock time. 65 | * Contributors: Giuseppe Silano, Luigi Iannelli 66 | 67 | 0.1.2 (2018-05-15) 68 | ------------------ 69 | * Added the basic.world file 70 | * The launch files have been modified considering the ROS package architecture 71 | * The iteration number has been moved from 1000 to 50 in order to speed up the simulation 72 | * Contributors: Giuseppe Silano, Luigi Iannelli 73 | 74 | 0.1.1 (2018-05-13) 75 | ------------------ 76 | * Fixed issues in the control law development 77 | * Contributors: Giuseppe Silano, Pasquale Oppido, Luigi Iannelli 78 | 79 | 0.1.0 (2018-04-30) 80 | ------------------ 81 | * Initial Ubuntu package release 82 | * Contributors: Giuseppe Silano, Pasquale Oppido, Luigi Iannelli 83 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | preferred-citation: 4 | type: conference 5 | authors: 6 | - family-names: "Silano" 7 | given-names: "Giuseppe" 8 | orcid: "https://orcid.org/0000-0002-6816-6002" 9 | - family-names: "Oppido" 10 | given-names: "Pasquale" 11 | - family-names: "Iannelli" 12 | given-names: "Luigi" 13 | orcid: "https://orcid.org/0000-0003-2034-0005" 14 | doi: "10.1109/SMC.2019.8914154" 15 | title: "Software-in-the-loop simulation for improving flight control system design: a quadrotor case study" 16 | publisher: "IEEE" 17 | preprint: "https://giuseppesilano.net/publications/smc19.pdf" 18 | booktitle: "2019 IEEE International Conference on Systems, Man, and Cybernetics (SMC)" 19 | link: "https://ieeexplore.ieee.org/document/8914154" 20 | start: 466 # First page number 21 | end: 471 # Last page number 22 | year: 2019 23 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bebop_simulator) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | geometry_msgs 8 | gazebo_msgs 9 | mav_msgs 10 | nav_msgs 11 | std_msgs 12 | roscpp 13 | sensor_msgs 14 | cmake_modules 15 | ) 16 | 17 | find_package( 18 | Eigen REQUIRED 19 | ) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} 23 | LIBRARIES position_controller extendedKalmanFilter position_controller_with_bebop waypoint_filter 24 | CATKIN_DEPENDS geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs std_msgs 25 | DEPENDS Eigen 26 | ) 27 | 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ${Eigen_INCLUDE_DIRS} 33 | ) 34 | 35 | add_library(position_controller 36 | src/library/position_controller.cpp 37 | ) 38 | 39 | add_library(position_controller_with_bebop 40 | src/library/position_controller_with_bebop.cpp 41 | ) 42 | 43 | add_library(extendedKalmanFilter 44 | src/library/extendedKalmanFilter.cpp 45 | ) 46 | 47 | add_library(waypoint_filter 48 | src/library/waypoint_filter.cpp 49 | ) 50 | 51 | target_link_libraries(position_controller ${catkin_LIBRARIES}) 52 | add_dependencies(position_controller ${catkin_EXPORTED_TARGETS}) 53 | 54 | target_link_libraries(position_controller_with_bebop ${catkin_LIBRARIES}) 55 | add_dependencies(position_controller_with_bebop ${catkin_EXPORTED_TARGETS}) 56 | 57 | target_link_libraries(extendedKalmanFilter ${catkin_LIBRARIES}) 58 | add_dependencies(extendedKalmanFilter ${catkin_EXPORTED_TARGETS}) 59 | 60 | target_link_libraries(waypoint_filter ${catkin_LIBRARIES}) 61 | add_dependencies(waypoint_filter ${catkin_EXPORTED_TARGETS}) 62 | 63 | add_executable(position_controller_node src/nodes/position_controller_node.cpp) 64 | add_dependencies(position_controller_node ${catkin_EXPORTED_TARGETS}) 65 | target_link_libraries(position_controller_node 66 | position_controller extendedKalmanFilter waypoint_filter ${catkin_LIBRARIES}) 67 | 68 | add_executable(position_controller_with_bebop_node src/nodes/position_controller_with_bebop_node.cpp) 69 | add_dependencies(position_controller_with_bebop_node ${catkin_EXPORTED_TARGETS}) 70 | target_link_libraries(position_controller_with_bebop_node 71 | position_controller_with_bebop extendedKalmanFilter waypoint_filter ${catkin_LIBRARIES}) 72 | 73 | add_executable(hovering_example src/nodes/hovering_example.cpp) 74 | target_link_libraries(hovering_example ${catkin_LIBRARIES}) 75 | add_dependencies(hovering_example ${catkin_EXPORTED_TARGETS}) 76 | 77 | add_executable(waypoint_example src/nodes/waypoint_example.cpp) 78 | target_link_libraries(waypoint_example ${catkin_LIBRARIES}) 79 | add_dependencies(waypoint_example ${catkin_EXPORTED_TARGETS}) 80 | 81 | add_executable(quaternion_to_rpy src/nodes/quaternion_to_rpy.cpp) 82 | target_link_libraries(quaternion_to_rpy ${catkin_LIBRARIES}) 83 | add_dependencies(quaternion_to_rpy ${catkin_EXPORTED_TARGETS}) 84 | 85 | install(TARGETS position_controller extendedKalmanFilter position_controller_with_bebop waypoint_filter 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | ) 88 | 89 | install(TARGETS position_controller position_controller_with_bebop_node hovering_example waypoint_example quaternion_to_rpy 90 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 93 | ) 94 | 95 | install( 96 | DIRECTORY include/${PROJECT_NAME}/ 97 | DIRECTORY include/bebop_simulator_msgs/ 98 | DIRECTORY include/bebop_msgs/ 99 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 100 | FILES_MATCHING PATTERN "*.h" 101 | ) 102 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Contributing 2 | ============ 3 | 4 | 👍🎉 Thanks a lot for considering contributing 🎉👍 5 | 6 | We welcome and encourage contribution. There is many way to contribute: you can 7 | write bug report, contribute code or documentation (please, take a look at the 8 | [Wiki Section](https://github.com/gsilano/BebopS/wiki)). 9 | 10 | ## Working on your first Pull Request? 11 | 12 | You can learn how from this *free* series [How to Contribute to an Open Source Project on GitHub](https://egghead.io/series/how-to-contribute-to-an-open-source-project-on-github) 13 | 14 | ## Reporting issues 15 | 16 | When reporting issues the more information you can supply the better. 17 | 18 | If it is an issue with building the ROS package, indicate your environment (Indigo 19 | Igloo or Kinetic Kame versions of ROS) like operating system (Ubuntu or MacOS). 20 | 21 | ## Improvements request and proposal 22 | 23 | Feel free to make an issue to request a new functionality. 24 | 25 | ## Contributing code/Pull-Request 26 | 27 | We welcome contribution, this can be done by starting a pull-request. 28 | You can contribute by updating the ROS package content or styling, code, or both. 29 | 30 | ### Contributing content or styling 31 | 32 | There is a couple of basic requirement for us to merge the pull request: 33 | - The pull request must pass the automated test (a TravisCI script will check if everything is OK) 34 | 35 | ### Contributing code 36 | 37 | If the change is big, typically if the change span to more than one file, consider starting an issue first to discuss the improvement. 38 | This will makes it much easier to make the change fit well into the software. 39 | 40 | There is some basic requirement for us to merge a pull request: 41 | - Describe the change (a bot will remind you in case you forget it) 42 | - Refer to any issues it effects 43 | - Separate one pull request per functionality: if you start writing "and" in the feature description consider if it could be separated in two pull requests. 44 | - Write a test for any new function or liquid plugin you are writing 45 | - The pull request must pass the automated test (as described previously) 46 | 47 | In your code: 48 | - Make sure the coding style of your code follows the style of the file (take a look at the [Developer Section](https://github.com/gsilano/BebopS/wiki#for-developers) into the first page of the repository Wiki page). 49 | 50 | ## Workflow 51 | 52 | ### Stable branch: `master` 53 | 54 | The `master` branch is stable and **should not receive new features**. Only **bug fixes** are accepted. 55 | 56 | This is the typical workflow to fix a bug in the master branch. 57 | 58 | * Identify a bug that does not require breaking changes of the API/ABI. 59 | * Open an issue on github. 60 | * Add some label (FIXME which label?). 61 | * Assign the issue to yourself. 62 | * Create a new branch starting from the `master` branch: 63 | 64 | ``` 65 | git fetch origin 66 | git checkout -b origin/master 67 | ``` 68 | 69 | * Fix the bug and make one or more commits. 70 | * [Push the branch on your fork and create a pull request](https://help.github.com/categories/collaborating-on-projects-using-pull-requests/). 71 | * Wait for someone else to review your fix and merge your pull request. 72 | * Your fix is now in the `master` branch, now you need to port it to the `dev` 73 | branch. 74 | * Ensure that your branches are in sync with `origin`: 75 | 76 | ``` 77 | git checkout master 78 | git pull --rebase origin master 79 | git checkout dev 80 | git pull --rebase origin dev 81 | ``` 82 | 83 | * Merge master into devel and eventually fix the conflicts. 84 | 85 | ``` 86 | git merge master 87 | ``` 88 | 89 | ##### Work in progress PR 90 | As final note, in case you need to start a PR but you deem it still **work-in-progress** and do not want anyone to merge it by mistake, do the following: 91 | - Put `[WIP]` at the beginning of the PR title. 92 | - Mark the PR with the label `"Status: In Progress"`. 93 | 94 | Once you are happy about your work, just remove the `[WIP]` tag as well as the label, and drop a message within the PR to notify the community that reviews are welcome and merging is now possible. 95 | 96 | ### Development branch: `dev` 97 | 98 | 99 | We use the branch `dev` to collect the ongoing work, which is given in terms of **new features** and **bug fixes**. 100 | 101 | When we introduce a new feature that will cause downstream projects to be aware of such update, we do increase the tweak number (always sticking to _odd numbers_). 102 | 103 | When we decide to publish these new features in a new software release (roughly each _3 months_), we merge the new modifications into `master`, doing: 104 | 105 | ```sh 106 | git checkout master 107 | git merge --no-ff dev 108 | git push origin master 109 | ``` 110 | -------------------------------------------------------------------------------- /config/defaults.yaml: -------------------------------------------------------------------------------- 1 | states: 2 | enable_commonstate_batterystatechanged: true 3 | enable_commonstate_wifisignalchanged: true 4 | enable_overheatstate_overheatchanged: true 5 | enable_controllerstate_ispilotingchanged: true 6 | enable_mavlinkstate_mavlinkfileplayingstatechanged: true 7 | enable_mavlinkstate_mavlinkplayerrorstatechanged: true 8 | enable_flightplanstate_availabilitystatechanged: true 9 | enable_flightplanstate_componentstatelistchanged: true 10 | enable_pilotingstate_altitudechanged: true 11 | enable_pilotingstate_flattrimchanged: true 12 | enable_pilotingstate_flyingstatechanged: true 13 | enable_pilotingstate_navigatehomestatechanged: true 14 | enable_pilotingstate_positionchanged: true 15 | enable_pilotingstate_speedchanged: true 16 | enable_pilotingstate_attitudechanged: true 17 | enable_pilotingstate_positionchanged: true 18 | enable_altitudechanged: true 19 | enable_autotakeoffmodechanged: true 20 | enable_mediastreamingstate_videoenablechanged: true 21 | enable_camerastate_orientation: true 22 | enable_gpsstate_numberofsatellitechanged: true 23 | enable_numberofsatellitechanged: true 24 | 25 | reset_settings: false 26 | publish_odom_tf: true 27 | odom_frame_id: "odom" 28 | cmd_vel_timeout: 0.2 29 | -------------------------------------------------------------------------------- /config/wind_turbine.yaml: -------------------------------------------------------------------------------- 1 | wind_turbine: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 100 6 | 7 | # Velocity controller --------------------------------------- 8 | wind_turbine_velocity_controller: 9 | type: effort_controllers/JointVelocityController 10 | joint: joint_wind_turbine_rotor 11 | pid: {p: 0.25, i: 0, d: 0.0} -------------------------------------------------------------------------------- /data/bebop2_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | image_width: 856 2 | image_height: 480 3 | camera_name: bebop_front 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [537.292878, 0.000000, 427.331854, 0.000000, 527.000348, 240.226888, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.004974, -0.000130, -0.001212, 0.002192, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [539.403503, 0.000000, 429.275072, 0.000000, 0.000000, 529.838562, 238.941372, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /include/bebop_msgs/default_topics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef BEBOP_DEFAULT_TOPICS_H_ 20 | #define BEBOP_DEFAULT_TOPICS_H_ 21 | 22 | namespace bebop_msgs { 23 | namespace default_topics { 24 | 25 | static constexpr char BEBOP_PARAMETERS[] = "bebop_driver/parameter_updates"; 26 | 27 | static constexpr char ODOM[] = "odom"; 28 | static constexpr char IMAGE_RAW[] = "image_raw"; 29 | static constexpr char BEBOP_FRONT[] = "bebop_front"; 30 | 31 | static constexpr char MEDIA_REC_PICT_STATE[] = "states/ardrone3/MediaRecordState/PictureStateChanged"; 32 | static constexpr char MEDIA_REC_VIDEO_RESOL[] = "states/ardrone3/MediaRecordState/VideoResolutionState"; 33 | static constexpr char MEDIA_REC_VIDEO_STATE_2[] = "states/ardrone3/MediaRecordState/VideoStateChangedV2"; 34 | static constexpr char MEDIA_REC_PICT_STATE_2[] = "states/ardrone3/MediaRecordState/PictureStateChangedV2"; 35 | static constexpr char MEDIA_REC_VIDEO_STATE[] = "states/ardrone3/MediaRecordState/VideoStateChanged"; 36 | 37 | static constexpr char ACCES_CONCT_LIST[] = "states/ardrone3/AccessoryState/ConnectedAccessories"; 38 | 39 | static constexpr char PROSTATE_FEATURE[] = "states/ardrone3/PROState/Features"; 40 | 41 | static constexpr char GPS_HOME_TYPE_CHOSEN[] = "states/ardrone3/GPSState/HomeTypeChosenChanged"; 42 | static constexpr char GPS_HOME_TYPE_AVAIL[] = "states/ardrone3/GPSState/HomeTypeAvailabilityChanged"; 43 | static constexpr char GPS_STATES_NUMB_SAT[] = "states/ardrone3/GPSState/NumberOfSatelliteChanged"; 44 | 45 | static constexpr char ANTI_FLICK_MODE_CHNG[] = "states/ardrone3/AntiflickeringState/modeChanged"; 46 | static constexpr char ANTI_FLICK_ELECT_FREQ[] = "states/ardrone3/AntiflickeringState/electricFrequencyChanged"; 47 | 48 | static constexpr char CAMERA_VELOCITY_RANGE[] = "states/ardrone3/CameraState/VelocityRange"; 49 | static constexpr char CAMERA_DEFAULT_CAMERA_ORIEN_V2[] = "states/ardrone3/CameraState/defaultCameraOrientationV2"; 50 | static constexpr char CAMERA_ORIENTATION_V2[] = "states/ardrone3/CameraState/OrientationV2"; 51 | static constexpr char CAMERA_DEFAULT_CAMERA_ORIEN[] = "states/ardrone3/CameraState/defaultCameraOrientation"; 52 | static constexpr char CAMERA_ORIENTATION[] = "states/ardrone3/CameraState/Orientation"; 53 | 54 | static constexpr char MEDIA_STREAMING_VIDEO_STREAM[] = "states/ardrone3/MediaStreamingState/VideoStreamModeChanged"; 55 | static constexpr char MEDIA_STREAMING_VIDEO[] = "states/ardrone3/MediaStreamingState/VideoEnableChanged"; 56 | 57 | static constexpr char NETWORK_ALL_WIFI_AUTH[] = "states/ardrone3/NetworkState/AllWifiAuthChannelChanged"; 58 | static constexpr char NETWORK_WIFI_AUTH[] = "states/ardrone3/NetworkState/WifiAuthChannelListChanged"; 59 | static constexpr char NETWORK_ALL_WIFI_SCAN[] = "states/ardrone3/NetworkState/AllWifiScanChanged"; 60 | static constexpr char NETWORK_WIFI_SCAN[] = "states/ardrone3/NetworkState/WifiScanListChanged"; 61 | 62 | static constexpr char PILOTING_STATES_MOVE_TO_CHND[] = "states/ardrone3/PilotingState/moveToChanged"; 63 | static constexpr char PILOTING_STATES_AIRSPEED_CHND[] = "states/ardrone3/PilotingState/AirSpeedChanged"; 64 | static constexpr char PILOTING_STATES_LAND_STATE_CHND[] = "states/ardrone3/PilotingState/LandingStateChanged"; 65 | static constexpr char PILOTING_STATES_GPS_LOC_CHND[] = "states/ardrone3/PilotingState/GpsLocationChanged"; 66 | static constexpr char PILOTING_STATES_ALTITUDE_CHND[] = "states/ardrone3/PilotingState/AltitudeChanged"; 67 | static constexpr char PILOTING_STATES_AUTO_TAKEOFF_MODE[] = "states/ardrone3/PilotingState/AutoTakeOffModeChanged"; 68 | static constexpr char PILOTING_STATES_ATT_CHND[] = "states/ardrone3/PilotingState/AttitudeChanged"; 69 | static constexpr char PILOTING_STATES_SPEED_CHND[] = "states/ardrone3/PilotingState/SpeedChanged"; 70 | static constexpr char PILOTING_STATES_POS_CHND[] = "states/ardrone3/PilotingState/PositionChanged"; 71 | static constexpr char PILOTING_STATES_NAV_HOME[] = "states/ardrone3/PilotingState/NavigateHomeStateChanged"; 72 | static constexpr char PILOTING_STATES_ALERT_STATE[] = "states/ardrone3/PilotingState/AlertStateChanged"; 73 | static constexpr char PILOTING_STATES_FLYING[] = "states/ardrone3/PilotingState/FlyingStateChanged"; 74 | static constexpr char PILOTING_STATES_FLAT_TRIM[] = "states/ardrone3/PilotingState/FlatTrimChanged"; 75 | 76 | static constexpr char COMMON_STATES_BATTERY[] = "states/common/CommonState/BatteryStateChanged"; 77 | static constexpr char COMMON_STATES_MASS[] = "states/common/CommonState/MassStorageStateListChanged"; 78 | static constexpr char COMMON_STATES_MASS_AND_INFO[] = "states/common/CommonState/BatteryStateChanged"; 79 | static constexpr char COMMON_STATES_DATE[] = "states/common/CommonState/CurrentDateChanged"; 80 | static constexpr char COMMON_STATES_CURRENT_TIME[] = "states/common/CommonState/CurrentTimeChanged"; 81 | static constexpr char COMMON_STATES_MASS_REMANING[] = "states/common/CommonState/MassStorageInfoRemainingListChanged"; 82 | static constexpr char COMMON_STATES_WIFI_SIGNAL[] = "states/common/CommonState/WifiSignalChanged"; 83 | static constexpr char COMMON_STATES_SENSOR_STATE[] = "states/common/CommonState/SensorsStatesListChanged"; 84 | static constexpr char COMMON_STATES_PRODUCT_MODEL[] = "states/common/CommonState/ProductModel"; 85 | static constexpr char COMMON_STATES_COUNTRY_LIST[] = "states/common/CommonState/CountryListKnown"; 86 | static constexpr char COMMON_STATES_MASS_CHANGED_CONTENT[] = "states/common/CommonState/DeprecatedMassStorageContentChanged"; 87 | static constexpr char COMMON_STATES_MASS_CONTENT[] = "states/common/CommonState/MassStorageContent"; 88 | static constexpr char COMMON_STATES_MASS_CONTENT_CURRENT_TIME[] = "states/common/CommonState/MassStorageContentForCurrentRun"; 89 | static constexpr char COMMON_STATES_VIDEO_RECORDING_TIME_STAMP[] = "states/common/CommonState/VideoRecordingTimestamp"; 90 | 91 | static constexpr char COMMON_STATES_OVER_HEAT[] = "states/common/OverHeatState/OverHeatChanged"; 92 | static constexpr char COMMON_STATES_OVER_HEAT_REGULATION[] = "states/common/OverHeatState/OverHeatRegulationChanged"; 93 | 94 | static constexpr char COMMON_STATES_MAVLINK_STATE[] = "states/common/MavlinkState/MavlinkFilePlayingStateChanged"; 95 | static constexpr char COMMON_STATES_MAVLINK_ERROR_STATE[] = "states/common/MavlinkState/MavlinkPlayErrorStateChanged"; 96 | static constexpr char COMMON_STATES_MISSION_TIME_EXECUTED[] = "states/common/MavlinkState/MissionItemExecuted"; 97 | 98 | static constexpr char COMMON_STATES_MISSION_CALIBRATION[] = "states/common/CalibrationState/MagnetoCalibrationStateChanged"; 99 | static constexpr char COMMON_STATES_MISSION_MAGNETO_CAL[] = "states/common/CalibrationState/MagnetoCalibrationRequiredState"; 100 | static constexpr char COMMON_STATES_MISSION_MAGNETO_AXIS_CAL[] = "states/common/CalibrationState/MagnetoCalibrationAxisToCalibrateChanged"; 101 | static constexpr char COMMON_STATES_MAGNETO_CAL_STARTED[] = "states/common/CalibrationState/MagnetoCalibrationStartedChanged"; 102 | static constexpr char COMMON_STATES_PILOT_CALIBRATION[] = "states/common/CalibrationState/PitotCalibrationStateChanged"; 103 | 104 | static constexpr char COMMON_STATES_AVAIL_STATE_CND[] = "states/common/FlightPlanState/AvailabilityStateChanged"; 105 | static constexpr char COMMON_STATES_COMP_STATE_LIS[] = "states/common/FlightPlanState/ComponentStateListChanged"; 106 | static constexpr char COMMON_STATES_LOCK_STATE_CND[] = "states/common/FlightPlanState/LockStateChanged"; 107 | 108 | static constexpr char COMMON_STATES_ARL_LIB_CTRL_CMD[] = "states/common/ARLibsVersionsState/ControllerLibARCommandsVersion"; 109 | static constexpr char COMMON_STATES_ARL_LIB_SKY_CTRL[] = "states/common/ARLibsVersionsState/SkyControllerLibARCommandsVersion"; 110 | static constexpr char COMMON_STATES_ARL_LIB_DEV_VERS[] = "states/common/ARLibsVersionsState/DeviceLibARCommandsVersion"; 111 | 112 | static constexpr char COMMON_STATES_AUDIO_STREAM[] = "states/common/AudioState/AudioStreamingRunning"; 113 | 114 | static constexpr char COMMON_STATES_INTENSITY_CHANGED[] = "states/common/HeadlightsState/intensityChanged"; 115 | 116 | static constexpr char COMMON_STATES_ANIM_STATE[] = "states/common/AnimationsState/List"; 117 | 118 | static constexpr char COMMON_STATES_ACCES_LIST_SUP[] = "states/common/AccessoryState/SupportedAccessoriesListChanged"; 119 | static constexpr char COMMON_STATES_ACCES_CONF_CHA[] = "states/common/AccessoryState/AccessoryConfigChanged"; 120 | static constexpr char COMMON_STATES_ACCES_CONF_MOD[] = "states/common/AccessoryState/AccessoryConfigModificationEnabled"; 121 | 122 | static constexpr char COMMON_STATES_MAX_CHARGE_RATE[] = "states/common/ChargerState/MaxChargeRateChanged"; 123 | static constexpr char COMMON_STATES_CURRENT_CHARGE_STATE[] = "states/common/ChargerState/CurrentChargeStateChanged"; 124 | static constexpr char COMMON_STATES_LAST_CHARGE_RATE[] = "states/common/ChargerState/LastChargeRateChanged"; 125 | static constexpr char COMMON_STATES_CHARING_INFO[] = "states/common/ChargerState/ChargingInfo"; 126 | 127 | static constexpr char COMMON_STATES_RUN_ID[] = "states/common/RunState/RunIdChanged"; 128 | 129 | static constexpr char CAMERA_CONTROL[] = "camera_control"; 130 | static constexpr char FLAT_TRIM[] = "flattrim"; 131 | static constexpr char FLIP[] = "flip"; 132 | static constexpr char SNAPSHOT[] = "snapshot"; 133 | static constexpr char SET_EXPOSURE[] = "set_exposure"; 134 | static constexpr char RECORD[] = "record"; 135 | 136 | static constexpr char AUTOFLIGHT_NAVIGATE_HOME[] = "autoflight/navigate_home"; 137 | static constexpr char AUTOFLIGHT_START[] = "autoflight/start"; 138 | static constexpr char AUTOFLIGHT_STOP[] = "autoflight/stop"; 139 | static constexpr char AUTOFLIGHT_PAUSE[] = "autoflight/pause"; 140 | 141 | static constexpr char TAKE_OFF[] = "takeoff"; 142 | static constexpr char LAND[] = "land"; 143 | static constexpr char RESET[] = "reset"; 144 | static constexpr char COMMAND_VEL[] = "cmd_vel"; 145 | 146 | } // end namespace default_topics 147 | } // end namespace bebop_msgs 148 | 149 | #endif /* BEBOP_DEFAULT_TOPICS_H_ */ 150 | -------------------------------------------------------------------------------- /include/bebop_simulator/MinMax.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | This software is provided 'as-is', without any express or implied warranty. 4 | In no event will the authors be held liable for any damages arising from the use of this software. 5 | Permission is granted to anyone to use this software for any purpose, 6 | including commercial applications, and to alter it and redistribute it freely, 7 | subject to the following restrictions: 8 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 9 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 10 | 3. This notice may not be removed or altered from any source distribution. 11 | */ 12 | 13 | 14 | 15 | #ifndef TF_MINMAX_H 16 | #define TF_MINMAX_H 17 | 18 | template 19 | TFSIMD_FORCE_INLINE const T& tfMin(const T& a, const T& b) 20 | { 21 | return a < b ? a : b ; 22 | } 23 | 24 | template 25 | TFSIMD_FORCE_INLINE const T& tfMax(const T& a, const T& b) 26 | { 27 | return a > b ? a : b; 28 | } 29 | 30 | 31 | template 32 | TFSIMD_FORCE_INLINE void tfSetMin(T& a, const T& b) 33 | { 34 | if (b < a) 35 | { 36 | a = b; 37 | } 38 | } 39 | 40 | template 41 | TFSIMD_FORCE_INLINE void tfSetMax(T& a, const T& b) 42 | { 43 | if (a < b) 44 | { 45 | a = b; 46 | } 47 | } 48 | 49 | 50 | #endif -------------------------------------------------------------------------------- /include/bebop_simulator/QuadWord.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | This software is provided 'as-is', without any express or implied warranty. 4 | In no event will the authors be held liable for any damages arising from the use of this software. 5 | Permission is granted to anyone to use this software for any purpose, 6 | including commercial applications, and to alter it and redistribute it freely, 7 | subject to the following restrictions: 8 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 9 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 10 | 3. This notice may not be removed or altered from any source distribution. 11 | */ 12 | 13 | 14 | #ifndef TF_QUADWORD_H 15 | #define TF_QUADWORD_H 16 | 17 | #include "Scalar.h" 18 | #include "MinMax.h" 19 | 20 | 21 | #if defined (__CELLOS_LV2) && defined (__SPU__) 22 | #include 23 | #endif 24 | 25 | 26 | namespace tf 27 | { 28 | /**@brief The QuadWord class is base class for Vector3 and Quaternion. 29 | * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. 30 | */ 31 | #ifndef USE_LIBSPE2 32 | ATTRIBUTE_ALIGNED16(class) QuadWord 33 | #else 34 | class QuadWord 35 | #endif 36 | { 37 | protected: 38 | 39 | #if defined (__SPU__) && defined (__CELLOS_LV2__) 40 | union { 41 | vec_float4 mVec128; 42 | tfScalar m_floats[4]; 43 | }; 44 | public: 45 | vec_float4 get128() const 46 | { 47 | return mVec128; 48 | } 49 | protected: 50 | #else //__CELLOS_LV2__ __SPU__ 51 | tfScalar m_floats[4]; 52 | #endif //__CELLOS_LV2__ __SPU__ 53 | 54 | public: 55 | 56 | 57 | /**@brief Return the x value */ 58 | TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; } 59 | /**@brief Return the y value */ 60 | TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; } 61 | /**@brief Return the z value */ 62 | TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; } 63 | /**@brief Set the x value */ 64 | TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;}; 65 | /**@brief Set the y value */ 66 | TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;}; 67 | /**@brief Set the z value */ 68 | TFSIMD_FORCE_INLINE void setZ(tfScalar z) { m_floats[2] = z;}; 69 | /**@brief Set the w value */ 70 | TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;}; 71 | /**@brief Return the x value */ 72 | TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; } 73 | /**@brief Return the y value */ 74 | TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; } 75 | /**@brief Return the z value */ 76 | TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; } 77 | /**@brief Return the w value */ 78 | TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; } 79 | 80 | //TFSIMD_FORCE_INLINE tfScalar& operator[](int i) { return (&m_floats[0])[i]; } 81 | //TFSIMD_FORCE_INLINE const tfScalar& operator[](int i) const { return (&m_floats[0])[i]; } 82 | ///operator tfScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. 83 | TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; } 84 | TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; } 85 | 86 | TFSIMD_FORCE_INLINE bool operator==(const QuadWord& other) const 87 | { 88 | return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); 89 | } 90 | 91 | TFSIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const 92 | { 93 | return !(*this == other); 94 | } 95 | 96 | /**@brief Set x,y,z and zero w 97 | * @param x Value of x 98 | * @param y Value of y 99 | * @param z Value of z 100 | */ 101 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z) 102 | { 103 | m_floats[0]=x; 104 | m_floats[1]=y; 105 | m_floats[2]=z; 106 | m_floats[3] = 0.f; 107 | } 108 | 109 | /* void getValue(tfScalar *m) const 110 | { 111 | m[0] = m_floats[0]; 112 | m[1] = m_floats[1]; 113 | m[2] = m_floats[2]; 114 | } 115 | */ 116 | /**@brief Set the values 117 | * @param x Value of x 118 | * @param y Value of y 119 | * @param z Value of z 120 | * @param w Value of w 121 | */ 122 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 123 | { 124 | m_floats[0]=x; 125 | m_floats[1]=y; 126 | m_floats[2]=z; 127 | m_floats[3]=w; 128 | } 129 | /**@brief No initialization constructor */ 130 | TFSIMD_FORCE_INLINE QuadWord() 131 | // :m_floats[0](tfScalar(0.)),m_floats[1](tfScalar(0.)),m_floats[2](tfScalar(0.)),m_floats[3](tfScalar(0.)) 132 | { 133 | } 134 | 135 | /**@brief Three argument constructor (zeros w) 136 | * @param x Value of x 137 | * @param y Value of y 138 | * @param z Value of z 139 | */ 140 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z) 141 | { 142 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; 143 | } 144 | 145 | /**@brief Initializing constructor 146 | * @param x Value of x 147 | * @param y Value of y 148 | * @param z Value of z 149 | * @param w Value of w 150 | */ 151 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 152 | { 153 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; 154 | } 155 | 156 | /**@brief Set each element to the max of the current values and the values of another QuadWord 157 | * @param other The other QuadWord to compare with 158 | */ 159 | TFSIMD_FORCE_INLINE void setMax(const QuadWord& other) 160 | { 161 | tfSetMax(m_floats[0], other.m_floats[0]); 162 | tfSetMax(m_floats[1], other.m_floats[1]); 163 | tfSetMax(m_floats[2], other.m_floats[2]); 164 | tfSetMax(m_floats[3], other.m_floats[3]); 165 | } 166 | /**@brief Set each element to the min of the current values and the values of another QuadWord 167 | * @param other The other QuadWord to compare with 168 | */ 169 | TFSIMD_FORCE_INLINE void setMin(const QuadWord& other) 170 | { 171 | tfSetMin(m_floats[0], other.m_floats[0]); 172 | tfSetMin(m_floats[1], other.m_floats[1]); 173 | tfSetMin(m_floats[2], other.m_floats[2]); 174 | tfSetMin(m_floats[3], other.m_floats[3]); 175 | } 176 | 177 | 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif //TFSIMD_QUADWORD_H -------------------------------------------------------------------------------- /include/bebop_simulator/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #ifndef INCLUDE_BEBOP_CONTROL_COMMON_H_ 22 | #define INCLUDE_BEBOP_CONTROL_COMMON_H_ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | namespace bebop_simulator { 32 | 33 | // Default values. 34 | static const std::string kDefaultNamespace = ""; 35 | static const std::string kDefaultCommandMotorSpeedTopic = 36 | mav_msgs::default_topics::COMMAND_ACTUATORS; // "command/motor_speed"; 37 | static const std::string kDefaultCommandMultiDofJointTrajectoryTopic = 38 | mav_msgs::default_topics::COMMAND_TRAJECTORY; // "command/trajectory" 39 | static const std::string kDefaultCommandRollPitchYawrateThrustTopic = 40 | mav_msgs::default_topics::COMMAND_ROLL_PITCH_YAWRATE_THRUST; 41 | // "command/roll_pitch_yawrate_thrust" 42 | static const std::string kDefaultImuTopic = 43 | mav_msgs::default_topics::IMU; // "imu 44 | static const std::string kDefaultOdometryTopic = 45 | mav_msgs::default_topics::ODOMETRY; // "odometry" 46 | 47 | struct EigenOdometry { 48 | EigenOdometry() 49 | : timeStampSec(-1), 50 | timeStampNsec(-1), 51 | position(0.0, 0.0, 0.0), 52 | orientation(Eigen::Quaterniond::Identity()), 53 | velocity(0.0, 0.0, 0.0), 54 | angular_velocity(0.0, 0.0, 0.0) {}; 55 | 56 | EigenOdometry(const double _timeStampSec, 57 | const double _timeStampNsec, 58 | const Eigen::Vector3d& _position, 59 | const Eigen::Quaterniond& _orientation, 60 | const Eigen::Vector3d& _velocity, 61 | const Eigen::Vector3d& _angular_velocity) { 62 | 63 | timeStampSec = _timeStampSec; 64 | timeStampNsec = _timeStampNsec; 65 | position = _position; 66 | orientation = _orientation; 67 | velocity = _velocity; 68 | angular_velocity = _angular_velocity; 69 | }; 70 | 71 | double timeStampSec; //nano seconds timeStamp Odometry message 72 | double timeStampNsec; //nano seconds timeStamp Odometry message 73 | Eigen::Vector3d position; 74 | Eigen::Quaterniond orientation; 75 | Eigen::Vector3d velocity; // Velocity is expressed in the Body frame! 76 | Eigen::Vector3d angular_velocity; 77 | }; 78 | 79 | inline void eigenOdometryFromMsg(const nav_msgs::OdometryConstPtr& msg, 80 | EigenOdometry* odometry) { 81 | odometry->timeStampSec = msg->header.stamp.sec; 82 | odometry->timeStampNsec = msg->header.stamp.nsec; 83 | odometry->position = mav_msgs::vector3FromPointMsg(msg->pose.pose.position); 84 | odometry->orientation = mav_msgs::quaternionFromMsg(msg->pose.pose.orientation); 85 | odometry->velocity = mav_msgs::vector3FromMsg(msg->twist.twist.linear); 86 | odometry->angular_velocity = mav_msgs::vector3FromMsg(msg->twist.twist.angular); 87 | } 88 | 89 | } 90 | #endif /* INCLUDE_BEBOP_CONTROL_COMMON_H_ */ 91 | -------------------------------------------------------------------------------- /include/bebop_simulator/extendedKalmanFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_ 20 | #define _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_ 21 | 22 | #include "bebop_simulator/transform_datatypes.h" 23 | #include "bebop_simulator/Matrix3x3.h" 24 | #include "bebop_simulator/Quaternion.h" 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "stabilizer_types.h" 35 | #include "filter_parameters.h" 36 | #include "common.h" 37 | #include "parameters_ros.h" 38 | 39 | namespace bebop_simulator { 40 | 41 | class ExtendedKalmanFilter { 42 | public: 43 | 44 | ExtendedKalmanFilter(); 45 | ~ExtendedKalmanFilter(); 46 | 47 | void EstimatorWithoutNoise(state_t *state_, EigenOdometry* odometry_, nav_msgs::Odometry* odometry_filtered); 48 | void EstimatorWithNoise(state_t *state_, EigenOdometry* odometry_, nav_msgs::Odometry* odometry_filtered); 49 | void SetThrustCommand(double u_T); 50 | void SetVehicleParameters(double m, double g); 51 | void SetFilterParameters(FilterParameters *filter_parameters_); 52 | 53 | //The function is used to disable the Extended Kalman Filter 54 | void Estimator(state_t *state_, EigenOdometry* odometry_); 55 | 56 | private: 57 | 58 | EigenOdometry odometry_private_; 59 | 60 | //Filter Vectors 61 | Eigen::VectorXf Xp_, Xe_, Hatx_; 62 | Eigen::MatrixXf P_, Pe_; 63 | Eigen::MatrixXf Rp_private_, Qp_private_; 64 | 65 | Eigen::MatrixXf A_private_, Qp_std_, Rp_std_, Hp_; 66 | 67 | std::normal_distribution distribution_; 68 | 69 | //Vehicle parameters 70 | double m_private_, g_private_; 71 | double u_T_private_; 72 | 73 | void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; 74 | void AttitudeAddingNoise(double *phin, double *thetan, double* psin, double phi, double theta, double psi); 75 | void SetOdometry(const EigenOdometry& odometry); 76 | void CorrectWithoutNoise(); 77 | void CorrectWithNoise(); 78 | void PredictWithoutNoise(); 79 | void PredictWithNoise(); 80 | 81 | }; 82 | 83 | } 84 | 85 | #endif // _ESTIMATOR_EXTENDED_KALMAN_FILTER_H_ 86 | 87 | 88 | -------------------------------------------------------------------------------- /include/bebop_simulator/filter_parameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef _FILTER_PARAMETERS_H_ 20 | #define _FILTER_PARAMETERS_H_ 21 | 22 | #include 23 | #include 24 | 25 | namespace bebop_simulator { 26 | 27 | static constexpr double DefaultDevX = 0.01; 28 | static constexpr double DefaultDevY = 0.01; 29 | static constexpr double DefaultDevZ = 0.01; 30 | 31 | static constexpr double DefaultDevVX = 0.01; 32 | static constexpr double DefaultDevVY = 0.01; 33 | static constexpr double DefaultDevVZ = 0.01; 34 | 35 | static constexpr double DefaultQpX = 1e-6; 36 | static constexpr double DefaultQpY = 1e-6; 37 | static constexpr double DefaultQpZ = 1e-6; 38 | 39 | static constexpr double DefaultQpVX = 1e-6; 40 | static constexpr double DefaultQpVY = 1e-6; 41 | static constexpr double DefaultQpVZ = 1e-6; 42 | 43 | class FilterParameters { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | FilterParameters(): 47 | dev_x_(DefaultDevX), 48 | dev_y_(DefaultDevY), 49 | dev_z_(DefaultDevZ), 50 | dev_vx_(DefaultDevVX), 51 | dev_vy_(DefaultDevVY), 52 | dev_vz_(DefaultDevVZ), 53 | Qp_x_(DefaultQpX), 54 | Qp_y_(DefaultQpY), 55 | Qp_z_(DefaultQpZ), 56 | Qp_vx_(DefaultQpVX), 57 | Qp_vy_(DefaultQpVY), 58 | Qp_vz_(DefaultQpVZ), 59 | Rp_(Eigen::MatrixXf::Zero(6,6)), 60 | Qp_(Eigen::MatrixXf::Identity(6,6)){ 61 | } 62 | 63 | Eigen::MatrixXf Rp_, Qp_; 64 | 65 | double dev_x_, Qp_x_; 66 | double dev_y_, Qp_y_; 67 | double dev_z_, Qp_z_; 68 | 69 | double dev_vx_, Qp_vx_; 70 | double dev_vy_, Qp_vy_; 71 | double dev_vz_, Qp_vz_; 72 | }; 73 | 74 | } 75 | 76 | #endif // _FILTER_PARAMETERS_H_ 77 | -------------------------------------------------------------------------------- /include/bebop_simulator/parameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef INCLUDE_BEBOP_CONTROL_PARAMETERS_H_ 20 | #define INCLUDE_BEBOP_CONTROL_PARAMETERS_H_ 21 | 22 | namespace bebop_simulator { 23 | 24 | // Default Bebop 2 parameters 25 | static constexpr double kDefaultMass = 0.5; 26 | static constexpr double kDefaultArmLength = 0.12905; 27 | static constexpr double kDefaultInertiaXx = 0.00389; 28 | static constexpr double kDefaultInertiaYy = 0.00389; 29 | static constexpr double kDefaultInertiaZz = 0.0078; 30 | static constexpr double kDefaultRotorForceConstant = 8.54858e-6; 31 | static constexpr double kDefaultRotorMomentConstant = 0.016; 32 | 33 | // Default physics parameters. 34 | static constexpr double kDefaultGravity = 9.81; 35 | 36 | class VehicleParameters { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | VehicleParameters() 40 | : mass_(kDefaultMass), 41 | gravity_(kDefaultGravity), 42 | inertia_(Eigen::Vector3d(kDefaultInertiaXx, kDefaultInertiaYy, 43 | kDefaultInertiaZz).asDiagonal()), 44 | bf_(kDefaultRotorForceConstant), 45 | bm_(kDefaultRotorMomentConstant), 46 | armLength_(kDefaultArmLength) {} 47 | 48 | double mass_; 49 | const double gravity_; 50 | Eigen::Matrix3d inertia_; 51 | double bm_; 52 | double bf_; 53 | double armLength_; 54 | }; 55 | 56 | } 57 | 58 | #endif /* INCLUDE_BEBOP_CONTROL_PARAMETERS_H_ */ 59 | -------------------------------------------------------------------------------- /include/bebop_simulator/parameters_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_BEBOP_CONTROL_PARAMETERS_ROS_H_ 2 | #define INCLUDE_BEBOP_CONTROL_PARAMETERS_ROS_H_ 3 | 4 | #include 5 | 6 | #include "parameters.h" 7 | 8 | namespace bebop_simulator { 9 | 10 | template inline void GetRosParameter(const ros::NodeHandle& nh, 11 | const std::string& key, 12 | const T& default_value, 13 | T* value) { 14 | ROS_ASSERT(value != nullptr); 15 | bool have_parameter = nh.getParam(key, *value); 16 | if (!have_parameter) { 17 | ROS_WARN_STREAM("[rosparam]: could not find parameter " << nh.getNamespace() 18 | << "/" << key << ", setting to default: " << default_value); 19 | *value = default_value; 20 | } 21 | } 22 | 23 | // The function allows to take data from the yaml file describing the quadrotor paramters 24 | inline void GetVehicleParameters(const ros::NodeHandle& nh, VehicleParameters* vehicle_parameters) { 25 | GetRosParameter(nh, "mass", 26 | vehicle_parameters->mass_, 27 | &vehicle_parameters->mass_); 28 | GetRosParameter(nh, "inertia/xx", 29 | vehicle_parameters->inertia_(0, 0), 30 | &vehicle_parameters->inertia_(0, 0)); 31 | GetRosParameter(nh, "inertia/xy", 32 | vehicle_parameters->inertia_(0, 1), 33 | &vehicle_parameters->inertia_(0, 1)); 34 | vehicle_parameters->inertia_(1, 0) = vehicle_parameters->inertia_(0, 1); 35 | GetRosParameter(nh, "inertia/xz", 36 | vehicle_parameters->inertia_(0, 2), 37 | &vehicle_parameters->inertia_(0, 2)); 38 | vehicle_parameters->inertia_(2, 0) = vehicle_parameters->inertia_(0, 2); 39 | GetRosParameter(nh, "inertia/yy", 40 | vehicle_parameters->inertia_(1, 1), 41 | &vehicle_parameters->inertia_(1, 1)); 42 | GetRosParameter(nh, "inertia/yz", 43 | vehicle_parameters->inertia_(1, 2), 44 | &vehicle_parameters->inertia_(1, 2)); 45 | vehicle_parameters->inertia_(2, 1) = vehicle_parameters->inertia_(1, 2); 46 | GetRosParameter(nh, "inertia/zz", 47 | vehicle_parameters->inertia_(2, 2), 48 | &vehicle_parameters->inertia_(2, 2)); 49 | GetRosParameter(nh, "bf", 50 | vehicle_parameters->bf_, 51 | &vehicle_parameters->bf_); 52 | GetRosParameter(nh, "bm", 53 | vehicle_parameters->bm_, 54 | &vehicle_parameters->bm_); 55 | GetRosParameter(nh, "l", 56 | vehicle_parameters->armLength_, 57 | &vehicle_parameters->armLength_); 58 | } 59 | 60 | } 61 | 62 | #endif /* INCLUDE_BEBOP_CONTROL_PARAMETERS_ROS_H_ */ 63 | -------------------------------------------------------------------------------- /include/bebop_simulator/position_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef BEBOP_CONTROL_POSITION_CONTROLLER_H 20 | #define BEBOP_CONTROL_POSITION_CONTROLLER_H 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include "extendedKalmanFilter.h" 30 | #include "waypoint_filter.h" 31 | #include "waypointfilter_parameters.h" 32 | #include "filter_parameters.h" 33 | #include "stabilizer_types.h" 34 | #include "parameters.h" 35 | #include "common.h" 36 | 37 | #include 38 | 39 | using namespace std; 40 | 41 | namespace bebop_simulator { 42 | 43 | // Default values for the Parrot Bebop controller. For more information about the control architecture, please take a look 44 | // at the publications page into the Wiki section. 45 | static const Eigen::Vector2d kPDefaultXYController = Eigen::Vector2d(-26.4259, -26.3627); 46 | static const double kPDefaultAltitudeController = -27.2277; 47 | 48 | static const double kPDefaultRollController = -1.7514; 49 | static const double kPDefaultPitchController = -1.7513; 50 | static const double kPDefaultYawRateController = -14.3431; 51 | 52 | static const Eigen::Vector2d MuDefaultXYController = Eigen::Vector2d(1, 1); 53 | static const double MuDefaultAltitudeController = 1; 54 | 55 | static const double MuDefaultRollController = 0.0544; 56 | static const double MuDefaultPitchController = 0.0543; 57 | static const double MuDefaultYawRateController = 0.44; 58 | 59 | static const Eigen::Vector3d UqDefaultXYZ = Eigen::Vector3d(1.1810, 1.1810, 4.6697); 60 | 61 | class PositionControllerParameters { 62 | public: 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | PositionControllerParameters() 65 | : beta_xy_(kPDefaultXYController), 66 | beta_z_(kPDefaultAltitudeController), 67 | beta_phi_(kPDefaultRollController), 68 | beta_theta_(kPDefaultPitchController), 69 | beta_psi_(kPDefaultYawRateController), 70 | mu_xy_(MuDefaultXYController), 71 | mu_z_(MuDefaultAltitudeController), 72 | mu_theta_(MuDefaultPitchController), 73 | mu_phi_(MuDefaultRollController), 74 | mu_psi_(MuDefaultYawRateController), 75 | U_q_(UqDefaultXYZ){ 76 | } 77 | 78 | Eigen::Vector2d beta_xy_; 79 | double beta_z_; 80 | 81 | double beta_phi_; 82 | double beta_theta_; 83 | double beta_psi_; 84 | 85 | Eigen::Vector2d mu_xy_; 86 | double mu_z_; 87 | 88 | double mu_phi_; 89 | double mu_theta_; 90 | double mu_psi_; 91 | 92 | Eigen::Vector3d U_q_; 93 | }; 94 | 95 | class PositionController{ 96 | public: 97 | PositionController(); 98 | ~PositionController(); 99 | void CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities); 100 | 101 | void SetOdometry(const EigenOdometry& odometry); 102 | void SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory_positionControllerNode); 103 | void SetControllerGains(); 104 | void SetVehicleParameters(); 105 | void SetWaypointFilterParameters(); 106 | void SetFilterParameters(); 107 | void GetOdometry(nav_msgs::Odometry* odometry_filtered); 108 | void GetReferenceAngles(nav_msgs::Odometry* reference_angles); 109 | void GetTrajectory(nav_msgs::Odometry* smoothed_trajectory); 110 | void GetUTerrComponents(nav_msgs::Odometry* uTerrComponents); 111 | void SetLaunchFileParameters(); 112 | void GetVelocityAlongZComponents(nav_msgs::Odometry* zVelocity_components); 113 | void GetPositionAndVelocityErrors(nav_msgs::Odometry* positionAndVelocityErrors); 114 | void GetAngularAndAngularVelocityErrors(nav_msgs::Odometry* angularAndAngularVelocityErrors); 115 | 116 | PositionControllerParameters controller_parameters_; 117 | ExtendedKalmanFilter extended_kalman_filter_bebop_; 118 | VehicleParameters vehicle_parameters_; 119 | FilterParameters filter_parameters_; 120 | WaypointFilterParameters waypoint_filter_parameters_; 121 | WaypointFilter waypoint_filter_; 122 | 123 | //Launch file parameters 124 | std::string user_; 125 | double dataStoringTime_; 126 | bool dataStoring_active_; 127 | bool waypointFilter_active_; 128 | bool EKF_active_; 129 | 130 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 131 | private: 132 | //Boolean variables to active/unactive the controller and the data storage 133 | bool controller_active_; 134 | 135 | //Wall clock time offset variable 136 | double wallSecsOffset_; 137 | 138 | //Gazebo Message for attitude and position 139 | gazebo_msgs::GetWorldProperties my_messagePosition_; 140 | ros::NodeHandle clientHandlePosition_; 141 | ros::ServiceClient clientPosition_; 142 | 143 | ros::NodeHandle clientHandleAttitude_; 144 | ros::ServiceClient clientAttitude_; 145 | gazebo_msgs::GetWorldProperties my_messageAttitude_; 146 | 147 | //Sting vectors used to stare data 148 | std::vector listControlSignals_; 149 | std::vector listControlMixerTerms_; 150 | std::vector listPropellersAngularVelocities_; 151 | std::vector listReferenceAngles_; 152 | std::vector listVelocityErrors_; 153 | std::vector listDroneAttitude_; 154 | std::vector listTrajectoryErrors_; 155 | std::vector listAttitudeErrors_; 156 | std::vector listDerivativeAttitudeErrors_; 157 | std::vector listTimeAttitudeErrors_; 158 | std::vector listTimePositionErrors_; 159 | std::vector listDroneAngularVelocitiesABC_; 160 | std::vector listDroneTrajectoryReference_; 161 | std::vector listControlMixerTermsSaturated_; 162 | std::vector listControlMixerTermsUnsaturated_; 163 | std::vector listDroneLinearVelocitiesABC_; 164 | std::vector listDronePosition_; 165 | std::vector listControlMixerTermsUnSaturatedBefore_; 166 | 167 | //Controller gains 168 | double beta_x_, beta_y_, beta_z_; 169 | double beta_phi_, beta_theta_, beta_psi_; 170 | 171 | double alpha_x_, alpha_y_, alpha_z_; 172 | double alpha_phi_, alpha_theta_, alpha_psi_; 173 | 174 | double mu_x_, mu_y_, mu_z_; 175 | double mu_phi_, mu_theta_, mu_psi_; 176 | 177 | double lambda_x_, lambda_y_, lambda_z_; 178 | double K_x_1_, K_x_2_; 179 | double K_y_1_, K_y_2_; 180 | double K_z_1_, K_z_2_; 181 | 182 | //Position and linear velocity errors 183 | double e_x_; 184 | double e_y_; 185 | double e_z_; 186 | double dot_e_x_; 187 | double dot_e_y_; 188 | double dot_e_z_; 189 | 190 | //Attitude and angular velocity errors 191 | double e_phi_; 192 | double e_theta_; 193 | double e_psi_; 194 | double dot_e_phi_; 195 | double dot_e_theta_; 196 | double dot_e_psi_; 197 | 198 | //Vehicle parameters 199 | double bf_, m_, g_; 200 | double l_, bm_; 201 | double Ix_, Iy_, Iz_; 202 | 203 | ros::NodeHandle n1_; 204 | ros::NodeHandle n2_; 205 | ros::NodeHandle n3_; 206 | ros::Timer timer1_; 207 | ros::Timer timer2_; 208 | ros::Timer timer3_; 209 | 210 | //Callback functions to compute the errors among axis and angles 211 | void CallbackAttitude(const ros::TimerEvent& event); 212 | void CallbackPosition(const ros::TimerEvent& event); 213 | void CallbackSaveData(const ros::TimerEvent& event); 214 | 215 | nav_msgs::Odometry odometry_filtered_private_; 216 | 217 | state_t state_; 218 | control_t control_; 219 | mav_msgs::EigenTrajectoryPoint command_trajectory_; 220 | EigenOdometry odometry_; 221 | 222 | void SetOdometryEstimated(); 223 | void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; 224 | void AttitudeController(double* u_phi, double* u_theta, double* u_psi); 225 | void AngularVelocityErrors(double* dot_e_phi_, double* dot_e_theta_, double* dot_e_psi_); 226 | void AttitudeErrors(double* e_phi_, double* e_theta_, double* e_psi_); 227 | void PosController(double* u_T, double* phi_r, double* theta_r, double* u_x, double* u_y, double* u_z, double* u_Terr); 228 | void PositionErrors(double* e_x, double* e_y, double* e_z); 229 | void VelocityErrors(double* dot_e_x, double* dot_e_y, double* dot_e_z); 230 | 231 | }; 232 | 233 | } 234 | #endif // BEBOP_CONTROL_POSITION_CONTROLLER_H 235 | -------------------------------------------------------------------------------- /include/bebop_simulator/position_controller_with_bebop.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef BEBOP_CONTROL_POSITION_CONTROLLER_H 20 | #define BEBOP_CONTROL_POSITION_CONTROLLER_H 21 | 22 | #include "waypoint_filter.h" 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include "extendedKalmanFilter.h" 30 | #include "stabilizer_types.h" 31 | #include "parameters.h" 32 | #include "common.h" 33 | 34 | using namespace std; 35 | 36 | namespace bebop_simulator { 37 | 38 | // Default values for the Parrot Bebop controller. For more information about the control architecture, please take a look 39 | // at the publications page into the Wiki section. 40 | static const Eigen::Vector2d kPDefaultXYController = Eigen::Vector2d(-26.4259, -26.3627); 41 | static const double kPDefaultAltitudeController = -27.2277; 42 | 43 | static const double kPDefaultRollController = -1.7514; 44 | static const double kPDefaultPitchController = -1.7513; 45 | static const double kPDefaultYawRateController = -14.3431; 46 | 47 | static const Eigen::Vector2d MuDefaultXYController = Eigen::Vector2d(1, 1); 48 | static const double MuDefaultAltitudeController = 1; 49 | 50 | static const double MuDefaultRollController = 0.0544; 51 | static const double MuDefaultPitchController = 0.0543; 52 | static const double MuDefaultYawRateController = 0.44; 53 | 54 | static const Eigen::Vector3d UqDefaultXYZ = Eigen::Vector3d(1.1810, 1.1810, 4.6697); 55 | 56 | 57 | class PositionControllerParameters { 58 | public: 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 60 | PositionControllerParameters() 61 | : beta_xy_(kPDefaultXYController), 62 | beta_z_(kPDefaultAltitudeController), 63 | beta_phi_(kPDefaultRollController), 64 | beta_theta_(kPDefaultPitchController), 65 | beta_psi_(kPDefaultYawRateController), 66 | mu_xy_(MuDefaultXYController), 67 | mu_z_(MuDefaultAltitudeController), 68 | mu_theta_(MuDefaultPitchController), 69 | mu_phi_(MuDefaultRollController), 70 | mu_psi_(MuDefaultYawRateController), 71 | U_q_(UqDefaultXYZ){ 72 | } 73 | 74 | Eigen::Vector2d beta_xy_; 75 | double beta_z_; 76 | 77 | double beta_phi_; 78 | double beta_theta_; 79 | double beta_psi_; 80 | 81 | Eigen::Vector2d mu_xy_; 82 | double mu_z_; 83 | 84 | double mu_phi_; 85 | double mu_theta_; 86 | double mu_psi_; 87 | 88 | Eigen::Vector3d U_q_; 89 | }; 90 | 91 | class PositionControllerWithBebop{ 92 | public: 93 | PositionControllerWithBebop(); 94 | ~PositionControllerWithBebop(); 95 | void CalculateCommandSignals(geometry_msgs::Twist* ref_command_signals); 96 | 97 | void SetOdom(const EigenOdometry& odometry); 98 | void SetTrajectoryPoint(); 99 | void SetControllerGains(); 100 | void SetVehicleParameters(); 101 | void SetFilterParameters(); 102 | void GetOdometry(nav_msgs::Odometry* odometry_filtered); 103 | void GetReferenceAngles(nav_msgs::Odometry* reference_angles); 104 | void GetTrajectory(nav_msgs::Odometry* smoothed_trajectory); 105 | 106 | PositionControllerParameters controller_parameters_; 107 | ExtendedKalmanFilter extended_kalman_filter_bebop_; 108 | VehicleParameters vehicle_parameters_; 109 | FilterParameters filter_parameters_; 110 | WaypointFilter waypoint_filter_; 111 | 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | private: 114 | //Boolean variables to active/unactive the controller and the data storage 115 | bool controller_active_; 116 | bool stateEmergency_; 117 | 118 | //publisher 119 | ros::Publisher land_pub_; 120 | ros::Publisher reset_pub_; 121 | 122 | //Controller gains 123 | double beta_x_, beta_y_, beta_z_; 124 | double beta_phi_, beta_theta_, beta_psi_; 125 | 126 | double alpha_x_, alpha_y_, alpha_z_; 127 | double alpha_phi_, alpha_theta_, alpha_psi_; 128 | 129 | double mu_x_, mu_y_, mu_z_; 130 | double mu_phi_, mu_theta_, mu_psi_; 131 | 132 | //Position and linear velocity errors 133 | double e_x_; 134 | double e_y_; 135 | double e_z_; 136 | double dot_e_x_; 137 | double dot_e_y_; 138 | double dot_e_z_; 139 | 140 | //Attitude and angular velocity errors 141 | double e_phi_; 142 | double e_theta_; 143 | double e_psi_; 144 | double dot_e_phi_; 145 | double dot_e_theta_; 146 | double dot_e_psi_; 147 | 148 | //Global u_T 149 | double u_T_; 150 | 151 | //Vehicle parameters 152 | double bf_, m_, g_; 153 | double l_, bm_; 154 | double Ix_, Iy_, Iz_; 155 | 156 | //Controller interface with Bebop paramters 157 | double e_z_sum_, vel_command_; 158 | 159 | ros::NodeHandle n1_; 160 | ros::NodeHandle n2_; 161 | ros::NodeHandle n3_; 162 | ros::NodeHandle n4_; 163 | ros::Timer timer1_; 164 | ros::Timer timer2_; 165 | ros::Timer timer3_; 166 | 167 | //Callback functions to compute the errors among axis and angles 168 | void CallbackAttitude(const ros::TimerEvent& event); 169 | void CallbackPosition(const ros::TimerEvent& event); 170 | void CallbackLand(const ros::TimerEvent& event); 171 | 172 | nav_msgs::Odometry odometry_filtered_private_; 173 | 174 | state_t state_; 175 | control_t control_; 176 | mav_msgs::EigenTrajectoryPoint command_trajectory_; 177 | EigenOdometry odometry_; 178 | 179 | void Emergency(); 180 | void LandEmergency(); 181 | void SetOdometryEstimated(); 182 | void CommandVelocity(double* vel_command); 183 | void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; 184 | void AttitudeController(double* u_phi, double* u_theta, double* u_psi); 185 | void AngularVelocityErrors(double* dot_e_phi_, double* dot_e_theta_, double* dot_e_psi_); 186 | void AttitudeErrors(double* e_phi_, double* e_theta_, double* e_psi_); 187 | void PosController(double* u_x, double* u_y, double* u_T, double* u_Terr); 188 | void PositionErrors(double* e_x, double* e_y, double* e_z); 189 | void VelocityErrors(double* dot_e_x, double* dot_e_y, double* dot_e_z); 190 | void ReferenceAngles(double* phi_r, double* theta_r); 191 | 192 | }; 193 | 194 | } 195 | #endif // BEBOP_CONTROL_POSITION_CONTROLLER_H 196 | -------------------------------------------------------------------------------- /include/bebop_simulator/stabilizer_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * Originally authored by BitCraze (https://github.com/bitcraze/crazyflie-firmware), commit #e6a3e26 on 22 Dec 2017 18 | * 19 | * || ____ _ __ 20 | * +------+ / __ )(_) /_______________ _____ ___ 21 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 22 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 23 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 24 | * 25 | * Crazyflie control firmware 26 | * 27 | * Copyright (C) 2011-2012 Bitcraze AB 28 | * 29 | * This program is free software: you can redistribute it and/or modify 30 | * it under the terms of the GNU General Public License as published by 31 | * the Free Software Foundation, in version 3. 32 | * 33 | * This program is distributed in the hope that it will be useful, 34 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | * GNU General Public License for more details. 37 | * 38 | * You should have received a copy of the GNU General Public License 39 | * along with this program. If not, see . 40 | * 41 | * stabilizer.h: Stabilizer orchestrator interface 42 | */ 43 | 44 | #ifndef __STABILIZER_TYPES_H__ 45 | #define __STABILIZER_TYPES_H__ 46 | 47 | #include 48 | 49 | namespace bebop_simulator { 50 | 51 | // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. 52 | #define RATE_1000_HZ 1000 53 | #define RATE_500_HZ 500 54 | #define RATE_250_HZ 250 55 | #define RATE_100_HZ 100 56 | #define RATE_50_HZ 50 57 | #define RATE_25_HZ 25 58 | 59 | #define RATE_MAIN_LOOP RATE_1000_HZ 60 | #define ATTITUDE_RATE RATE_500_HZ 61 | #define POSITION_RATE RATE_100_HZ 62 | 63 | #define RATE_DO_EXECUTE(RATE_HZ, TICK) ((TICK % (RATE_MAIN_LOOP / RATE_HZ)) == 0) 64 | 65 | 66 | /** Attitude in euler angle form */ 67 | typedef struct attitude_s { 68 | double roll; //phi 69 | double pitch; //theta 70 | double yaw; //psi 71 | } attitude_t; 72 | 73 | /* x,y,z vector */ 74 | struct vec3_s { 75 | double x; 76 | double y; 77 | double z; 78 | }; 79 | 80 | typedef struct vec3_s point_t; 81 | typedef struct vec3_s velocity_t; 82 | typedef struct vec3_s acc_t; 83 | 84 | /* Orientation as a quaternion */ 85 | typedef struct quaternion_s { 86 | union { 87 | struct { 88 | double q0; 89 | double q1; 90 | double q2; 91 | double q3; 92 | }; 93 | struct { 94 | double x; 95 | double y; 96 | double z; 97 | double w; 98 | }; 99 | }; 100 | } quaternion_t; 101 | 102 | 103 | typedef union Axis_s { 104 | struct { 105 | double x; 106 | double y; 107 | double z; 108 | }; 109 | double axis[3]; 110 | } Axis3f; 111 | 112 | 113 | typedef struct sensorData_s { 114 | Axis3f acc; // m/s^2 115 | Axis3f gyro; // rad/s 116 | } sensorData_t; 117 | 118 | typedef struct state_s { 119 | attitude_t attitude; // rad 120 | quaternion_t attitudeQuaternion; 121 | point_t position; // m 122 | velocity_t angularVelocity; // rad/s 123 | acc_t angularAcc; // m/s^2 124 | velocity_t linearVelocity; // m/s 125 | } state_t; 126 | 127 | typedef struct control_s { 128 | double phiR; // rad 129 | double thetaR; // rad 130 | double dotPsi; // rad/s 131 | double uT; // m/s^2 132 | } control_t; 133 | 134 | } 135 | 136 | #endif //__STABILIZER_TYPES_H__ 137 | 138 | -------------------------------------------------------------------------------- /include/bebop_simulator/waypoint_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef WAYPOINT_FILTER_H 20 | #define WAYPOINT_FILTER_H 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "stabilizer_types.h" 29 | 30 | #include "waypointfilter_parameters.h" 31 | 32 | using namespace std; 33 | 34 | namespace bebop_simulator { 35 | 36 | class WaypointFilter{ 37 | 38 | public: 39 | 40 | WaypointFilter(); 41 | ~WaypointFilter(); 42 | 43 | void SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory_positionControllerNode); 44 | void GetTrajectoryPoint(mav_msgs::EigenTrajectoryPoint* command_trajectory_positionController); 45 | void TrajectoryGeneration(); 46 | void SetParameters(WaypointFilterParameters *waypointFilter_parameters_); 47 | void Initialize(state_t state_); 48 | 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | private: 51 | 52 | double Tsf_, H_; 53 | bool filter_initialized_; 54 | 55 | mav_msgs::EigenTrajectoryPoint command_trajectory_private_; 56 | mav_msgs::EigenTrajectoryPoint command_trajectory_toSend_; 57 | 58 | }; 59 | 60 | } 61 | #endif // WAYPOINT_FILTER_H 62 | -------------------------------------------------------------------------------- /include/bebop_simulator/waypointfilter_parameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef _WAYPOINT_FILTER_PARAMETERS_H_ 19 | #define _WAYPOINT_FILTER_PARAMETERS_H_ 20 | 21 | #include 22 | #include 23 | 24 | namespace bebop_simulator { 25 | 26 | static constexpr double DefaultH = 10e-3; /* Sampling time [s] */ 27 | static constexpr double DefaultTsf = 1.5; /* Waypoint filter pole [s] */ 28 | 29 | class WaypointFilterParameters { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | WaypointFilterParameters() 33 | : tsf_(DefaultTsf), 34 | h_(DefaultH){ 35 | } 36 | 37 | double tsf_; 38 | double h_; 39 | }; 40 | 41 | } 42 | 43 | #endif // _WAYPOINT_FILTER_PARAMETERS_H_ 44 | -------------------------------------------------------------------------------- /include/bebop_simulator_msgs/default_topics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #ifndef BEBOPS_DEFAULT_MESSAGES_H_ 19 | #define BEBOPS_DEFAULT_MESSAGES_H_ 20 | 21 | namespace bebop_simulator_msgs { 22 | namespace default_topics { 23 | 24 | static constexpr char FILTERED_OUTPUT[] = "filteredOutput"; 25 | static constexpr char STATE_ERRORS[] = "stateErrors"; 26 | 27 | static constexpr char ODOMETRY_GT[] = "odometry_gt"; 28 | 29 | static constexpr char REFERENCE_ANGLES[] = "referenceAngles"; 30 | static constexpr char SMOOTHED_TRAJECTORY[] = "smoothedTrajectory"; 31 | 32 | static constexpr char U_TERR_COMPONENTS[] = "uTerrComponents"; 33 | 34 | static constexpr char Z_VELOCITY_COMPONENTS[] = "zVelocityComponents"; 35 | 36 | static constexpr char POSITION_AND_VELOCITY_ERRORS[] = "positionAndVelocityErrors"; 37 | 38 | static constexpr char ANGULAR_AND_ANGULAR_VELOCITY_ERRORS[] = "angularAndAngularVelocityErrors"; 39 | 40 | 41 | } // end namespace default_topics 42 | } // end namespace bebop_simulator_msgs 43 | 44 | #endif /* BEBOPS_DEFAULT_MESSAGES_H_ */ 45 | -------------------------------------------------------------------------------- /launch/bebop_without_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 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 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /launch/gazebo_bebop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /launch/multiple_bebop_without_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 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 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /launch/spawn_bebop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 43 | 44 | 45 | 46 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /launch/spawn_wind_turbine.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 25 | 26 | 27 | 28 | 29 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /launch/task1_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 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 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /launch/task1_world_with_bebop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /launch/task2_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /launch/task2_world_with_bebop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /launch/task3_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/wind_turbine_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /models/grass_plane/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MODEL_NAME grass_plane) 2 | set (files 3 | model.config 4 | model.sdf 5 | ) 6 | 7 | add_subdirectory(materials) 8 | 9 | install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/) 10 | -------------------------------------------------------------------------------- /models/grass_plane/materials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(scripts) 2 | add_subdirectory(textures) 3 | -------------------------------------------------------------------------------- /models/grass_plane/materials/scripts/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set (files 2 | grass.material 3 | ) 4 | 5 | install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/scripts/) 6 | -------------------------------------------------------------------------------- /models/grass_plane/materials/scripts/grass.material: -------------------------------------------------------------------------------- 1 | material vrc/grass 2 | { 3 | receive_shadows on 4 | technique 5 | { 6 | pass 7 | { 8 | ambient 0.8 1.0 0.8 1.0 9 | diffuse 0.8 1.0 0.8 1.0 10 | specular 0.1 0.1 0.1 1.0 12.5 11 | 12 | texture_unit 13 | { 14 | texture grass_dry.png 15 | filtering anistropic 16 | max_anisotropy 16 17 | } 18 | } 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /models/grass_plane/materials/textures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set (files 2 | grass_dry.png 3 | ) 4 | 5 | install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/textures) 6 | -------------------------------------------------------------------------------- /models/grass_plane/materials/textures/grass_dry.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gsilano/BebopS/07ef75dcb98413531a27f7c8cd7ffee3f8796dc2/models/grass_plane/materials/textures/grass_dry.png -------------------------------------------------------------------------------- /models/grass_plane/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | grass plane 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.com 11 | 12 | 13 | 14 | A grass textured plane. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /models/ground_plane_residential/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /models/ground_plane_residential/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /models/ground_plane_residential/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /models/ground_plane_residential/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ground Plane 5 | 1.0 6 | model-1_2.sdf 7 | model-1_3.sdf 8 | model-1_4.sdf 9 | model.sdf 10 | 11 | 12 | Nate Koenig 13 | nate@osrfoundation.org 14 | 15 | 16 | 17 | A simple ground plane. 18 | 19 | 20 | -------------------------------------------------------------------------------- /models/ground_plane_residential/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /models/occupancy_grid/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Occupancy Grid 4 | 1.0 5 | occupancy_grid.sdf 6 | 7 | 8 | Fadri Furrer 9 | ffurrer@gmail.com 10 | 11 | 12 | 13 | A simple world with some obstacles used for the EuRoC octomap challenge. 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/occupancy_grid/occupancy_grid.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 22 22 0.2 9 | 10 | 11 | 0 0 0.1 0 0 0 12 | 13 | 14 | 0 0 0.1 0 0 0 15 | 16 | 17 | 22 22 0.2 18 | 19 | 20 | 21 | 25 | 26 | 27 | 28 | 0 29 | 0 30 | 31 | 0 0 3.5 0 0 0 32 | 33 | 34 | 35 | 36 | 37 | 22 22 0.2 38 | 39 | 40 | 0 0 0.1 0 0 0 41 | 42 | 43 | 0 0 0.1 0 0 0 44 | 45 | 46 | 22 22 0.2 47 | 48 | 49 | 50 | 54 | 55 | 56 | 57 | 0 58 | 0 59 | 60 | 0 0 -0.2 0 0 0 61 | 62 | 63 | 64 | 65 | 66 | 20 1 3.5 67 | 68 | 69 | 0 0 1.75 0 0 0 70 | 71 | 72 | 0 0 1.75 0 0 0 73 | 74 | 75 | 20 1 3.5 76 | 77 | 78 | 79 | 83 | 84 | 85 | 86 | 0 87 | 0 88 | 89 | -10.5 0 0 0 0 -1.5708 90 | 91 | 92 | 93 | 94 | 95 | 22 1 3.5 96 | 97 | 98 | 0 0 1.75 0 0 0 99 | 100 | 101 | 0 0 1.75 0 0 0 102 | 103 | 104 | 22 1 3.5 105 | 106 | 107 | 108 | 112 | 113 | 114 | 115 | 0 116 | 0 117 | 118 | 0 -10.5 0 0 0 0 119 | 120 | 121 | 122 | 123 | 124 | 20 1 3.5 125 | 126 | 127 | 0 0 1.75 0 0 0 128 | 129 | 130 | 0 0 1.75 0 0 0 131 | 132 | 133 | 20 1 3.5 134 | 135 | 136 | 137 | 141 | 142 | 143 | 144 | 0 145 | 0 146 | 147 | 10.5 0 0 0 0 1.5708 148 | 149 | 150 | 151 | 152 | 153 | 22 1 3.5 154 | 155 | 156 | 0 0 1.75 0 0 0 157 | 158 | 159 | 0 0 1.75 0 0 0 160 | 161 | 162 | 22 1 3.5 163 | 164 | 165 | 166 | 170 | 171 | 172 | 173 | 0 174 | 0 175 | 176 | 0 10.5 0 0 0 0 177 | 178 | 179 | 180 | 181 | 182 | 0.2 20 2 183 | 184 | 185 | 0 0 1 0 0 0 186 | 187 | 188 | 0 0 1 0 0 0 189 | 190 | 191 | 0.2 20 2 192 | 193 | 194 | 195 | 199 | 200 | 201 | 202 | 0 203 | 0 204 | 205 | 2 0 0 0 0 0 206 | 207 | 208 | 209 | 210 | 211 | 0.2 3 3.5 212 | 213 | 214 | 0 0 1.75 0 0 0 215 | 216 | 217 | 0 0 1.75 0 0 0 218 | 219 | 220 | 0.2 3 3.5 221 | 222 | 223 | 224 | 228 | 229 | 230 | 231 | 0 232 | 0 233 | 234 | -2 1 0 0 0 1 235 | 236 | 237 | 238 | 239 | 240 | 0.2 4 3.5 241 | 242 | 243 | 0 0 1.75 0 0 0 244 | 245 | 246 | 0 0 1.75 0 0 0 247 | 248 | 249 | 0.2 4 3.5 250 | 251 | 252 | 253 | 257 | 258 | 259 | 260 | 0 261 | 0 262 | 263 | -4 -3 0 0 0 0 264 | 265 | 266 | 267 | 268 | 269 | 0.2 6 3.5 270 | 271 | 272 | 0 0 1.75 0 0 0 273 | 274 | 275 | 0 0 1.75 0 0 0 276 | 277 | 278 | 0.2 6 3.5 279 | 280 | 281 | 282 | 286 | 287 | 288 | 289 | 0 290 | 0 291 | 292 | -3 3 0 0 0 1.57 293 | 294 | 295 | 296 | 297 | 298 | 0.2 4 3.5 299 | 300 | 301 | 0 0 1.75 0 0 0 302 | 303 | 304 | 0 0 1.75 0 0 0 305 | 306 | 307 | 0.2 4 3.5 308 | 309 | 310 | 311 | 315 | 316 | 317 | 318 | 0 319 | 0 320 | 321 | -1 -2 0 0 0 0 322 | 323 | 1 324 | 325 | 326 | -------------------------------------------------------------------------------- /models/vrc_heightmap_1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MODEL_NAME vrc_heightmap_1) 2 | set (files 3 | model.config 4 | model.sdf 5 | ) 6 | 7 | add_subdirectory(materials) 8 | 9 | install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/) 10 | -------------------------------------------------------------------------------- /models/vrc_heightmap_1/materials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(textures) 2 | -------------------------------------------------------------------------------- /models/vrc_heightmap_1/materials/textures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set (files 2 | heightmap.png 3 | ) 4 | 5 | install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gazebo_models/environments/${MODEL_NAME}/materials/textures/) 6 | -------------------------------------------------------------------------------- /models/vrc_heightmap_1/materials/textures/heightmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gsilano/BebopS/07ef75dcb98413531a27f7c8cd7ffee3f8796dc2/models/vrc_heightmap_1/materials/textures/heightmap.png -------------------------------------------------------------------------------- /models/vrc_heightmap_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | VRC Heightmap 1 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.com 11 | 12 | 13 | 14 | Terrain for task 1 of VRC. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /models/vrc_heightmap_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://vrc_heightmap_1/materials/textures/heightmap.png 10 | 500 500 118 11 | 0 0 -15 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | file://media/materials/textures/dirt_diffusespecular.png 21 | file://media/materials/textures/flat_normal.png 22 | 5 23 | 24 | 25 | file://media/materials/textures/grass_diffusespecular.png 26 | file://media/materials/textures/flat_normal.png 27 | 5 28 | 29 | 30 | file://media/materials/textures/fungus_diffusespecular.png 31 | file://media/materials/textures/flat_normal.png 32 | 20 33 | 34 | 35 | 15 36 | 5 37 | 38 | 39 | 30 40 | 10 41 | 42 | model://vrc_heightmap_1/materials/textures/heightmap.png 43 | 500 500 118 44 | 0 0 -15 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /models/yosemite/materials/textures/texture_forest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gsilano/BebopS/07ef75dcb98413531a27f7c8cd7ffee3f8796dc2/models/yosemite/materials/textures/texture_forest.png -------------------------------------------------------------------------------- /models/yosemite/materials/textures/texture_mountains.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gsilano/BebopS/07ef75dcb98413531a27f7c8cd7ffee3f8796dc2/models/yosemite/materials/textures/texture_mountains.png -------------------------------------------------------------------------------- /models/yosemite/materials/textures/yosemite.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gsilano/BebopS/07ef75dcb98413531a27f7c8cd7ffee3f8796dc2/models/yosemite/materials/textures/yosemite.png -------------------------------------------------------------------------------- /models/yosemite/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Yosemite 4 | 1.0 5 | yosemite.sdf 6 | 7 | 8 | Pavel Vechersky 9 | pvechersky@gmail.com 10 | 11 | 12 | 13 | Part of the Yosemite terrain, imported from real-world evelation data. 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/yosemite/yosemite.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://yosemite/materials/textures/yosemite.png 10 | 1000 1000 125 11 | 100 0 -30 12 | 13 | 14 | 15 | 16 | 17 | 0.2 18 | 0.2 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | file://media/materials/textures/dirt_diffusespecular.png 28 | file://media/materials/textures/flat_normal.png 29 | 50 30 | 31 | 32 | file://media/materials/textures/terrain_detail.jpg 33 | file://media/materials/textures/flat_normal.png 34 | 75 35 | 36 | 37 | file://yosemite/materials/textures/texture_forest.png 38 | file://media/materials/textures/flat_normal.png 39 | 100 40 | 41 | 42 | file://yosemite/materials/textures/texture_mountains.png 43 | file://media/materials/textures/flat_normal.png 44 | 50 45 | 46 | 47 | 35 48 | 10 49 | 50 | 51 | 60 52 | 10 53 | 54 | 55 | 90 56 | 10 57 | 58 | model://yosemite/materials/textures/yosemite.png 59 | 1000 1000 125 60 | 100 0 -30 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bebop_simulator 4 | 1.0.2 5 | This package aims to simulate the behavior of the Parrot Bebop by using SIL methodologies. 6 | 7 | Giuseppe Silano 8 | Luigi Iannelli 9 | 10 | Giuseppe Silano 11 | Pasquale Oppido 12 | Luigi Iannelli 13 | Antun Ivanovic 14 | tomlogan501 15 | Collin-Thornton 16 | 17 | Apache 18 | 19 | https://github.com/gsilano/BebopS 20 | https://github.com/gsilano/BebopS/issues 21 | 22 | catkin 23 | 24 | 25 | gazebo_plugins 26 | rotors_gazebo_plugins 27 | gazebo_msgs 28 | geometry_msgs 29 | mav_msgs 30 | nav_msgs 31 | std_msgs 32 | bebop_driver 33 | roscpp 34 | sensor_msgs 35 | cmake_modules 36 | tf2 37 | tf 38 | 39 | 40 | gazebo_plugins 41 | rotors_gazebo_plugins 42 | gazebo_msgs 43 | dynamic_reconfigure 44 | geometry_msgs 45 | mav_msgs 46 | std_msgs 47 | nav_msgs 48 | bebop_driver 49 | roscpp 50 | sensor_msgs 51 | tf2 52 | tf 53 | 54 | 55 | -------------------------------------------------------------------------------- /resource/EKF_matrix.yaml: -------------------------------------------------------------------------------- 1 | # Tuning matrix of the Extended Kalman Filter (min 1e-5) 2 | Qp: {aa: 0.001, bb: 0.001, cc: 0.001, dd: 0.001, ee: 0.001, ff: 0.001} 3 | # Standard deviation of noise on positions and linear velocities 4 | dev_x: 0.01 5 | dev_y: 0.01 6 | dev_z: 0.01 7 | dev_vx: 0.01 8 | dev_vy: 0.01 9 | dev_vz: 0.01 10 | 11 | -------------------------------------------------------------------------------- /resource/bebop.yaml: -------------------------------------------------------------------------------- 1 | # Bebop vehicle parameters 2 | mass: 0.5 3 | inertia: {xx: 0.00389, xy: 0.0, xz: 0.0, yy: 0.00389, yz: 0.0, zz: 0.0078} 4 | bf: 8.54858e-6 5 | bm: 0.016 6 | l: 0.12905 7 | -------------------------------------------------------------------------------- /resource/controller_bebop.yaml: -------------------------------------------------------------------------------- 1 | # Position controller parameters 2 | beta_xy: {beta_x: -26.4259, beta_y: -26.3627} 3 | beta_z: {beta_z: -27.2277} 4 | beta_phi: {beta_phi: -1.7514} 5 | beta_theta: {beta_theta: -1.7513} 6 | beta_psi: {beta_psi: -14.3431} 7 | mu_xy: {mu_x: 1, mu_y: 1} 8 | mu_z: {mu_z: 1} 9 | mu_phi: {mu_phi: 0.0544} 10 | mu_theta: {mu_theta: 0.0543} 11 | mu_psi: {mu_psi: 0.44} 12 | U_xyz: {U_x: 1.1810, U_y: 1.1810, U_z: 4.6697} 13 | -------------------------------------------------------------------------------- /resource/waypoint_filter.yaml: -------------------------------------------------------------------------------- 1 | # Waypoint filter parameters 2 | Tsf: 1.5 # Waypoint filter time constant [s] 3 | H: 0.01 # Sampling time [s] 4 | 5 | -------------------------------------------------------------------------------- /resource/waypoints.txt: -------------------------------------------------------------------------------- 1 | 10 0.0 0.0 1.0 0.0 2 | 10 -1.0 -1.0 1.0 0.0 3 | 10 0.0 0.0 1.0 0.0 4 | -------------------------------------------------------------------------------- /resource/waypoints0.txt: -------------------------------------------------------------------------------- 1 | 5 0.0 0.0 1.0 0.0 2 | 5 0.0 -1.0 1.0 0.0 3 | 5 0.0 0.0 1.0 0.0 4 | 5 0.0 0.0 0.0 0.0 -------------------------------------------------------------------------------- /resource/waypoints1.txt: -------------------------------------------------------------------------------- 1 | 5 1.0 0.0 1.0 0.0 2 | 5 1.0 1.0 1.0 0.0 3 | 5 1.0 0.0 1.0 0.0 4 | 5 1.0 0.0 0.0 0.0 -------------------------------------------------------------------------------- /resource/waypoints2.txt: -------------------------------------------------------------------------------- 1 | 5 2.0 0.0 1.0 0.0 2 | 5 2.0 -1.0 1.0 0.0 3 | 5 2.0 0.0 1.0 0.0 4 | 5 2.0 0.0 0.0 0.0 -------------------------------------------------------------------------------- /resource/waypoints3.txt: -------------------------------------------------------------------------------- 1 | 5 3.0 0.0 1.0 0.0 2 | 5 3.0 1.0 1.0 0.0 3 | 5 3.0 0.0 1.0 0.0 4 | 5 3.0 0.0 0.0 0.0 -------------------------------------------------------------------------------- /rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Imu1 10 | - /Imu1/Status1 11 | - /RobotModel1 12 | - /RobotModel1/Status1 13 | Splitter Ratio: 0.47647058963775635 14 | Tree Height: 355 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: Image 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Class: rviz/Image 60 | Enabled: true 61 | Image Topic: /bebop/camera1/image_raw 62 | Max Value: 1 63 | Median window: 5 64 | Min Value: 0 65 | Name: Image 66 | Normalize Range: true 67 | Queue Size: 2 68 | Transport Hint: raw 69 | Unreliable: false 70 | Value: true 71 | - Alpha: 1 72 | Class: rviz_plugin_tutorials/Imu 73 | Color: 204; 51; 204 74 | Enabled: false 75 | History Length: 1 76 | Name: Imu 77 | Topic: /bebop/imu 78 | Unreliable: false 79 | Value: false 80 | - Angle Tolerance: 0.10000000149011612 81 | Class: rviz/Odometry 82 | Covariance: 83 | Orientation: 84 | Alpha: 0.5 85 | Color: 255; 255; 127 86 | Color Style: Unique 87 | Frame: Local 88 | Offset: 1 89 | Scale: 1 90 | Value: true 91 | Position: 92 | Alpha: 0.30000001192092896 93 | Color: 204; 51; 204 94 | Scale: 1 95 | Value: true 96 | Value: true 97 | Enabled: true 98 | Keep: 100 99 | Name: Odometry 100 | Position Tolerance: 0.10000000149011612 101 | Shape: 102 | Alpha: 1 103 | Axes Length: 1 104 | Axes Radius: 0.10000000149011612 105 | Color: 255; 25; 0 106 | Head Length: 0.30000001192092896 107 | Head Radius: 0.10000000149011612 108 | Shaft Length: 1 109 | Shaft Radius: 0.05000000074505806 110 | Value: Arrow 111 | Topic: /bebop/odometry 112 | Unreliable: false 113 | Value: true 114 | - Class: rviz/TF 115 | Enabled: true 116 | Frame Timeout: 15 117 | Frames: 118 | All Enabled: true 119 | bebop/base_link: 120 | Value: true 121 | world: 122 | Value: true 123 | Marker Scale: 1 124 | Name: TF 125 | Show Arrows: true 126 | Show Axes: true 127 | Show Names: true 128 | Tree: 129 | world: 130 | bebop/base_link: 131 | {} 132 | Update Interval: 0 133 | Value: true 134 | - Alpha: 1 135 | Class: rviz/RobotModel 136 | Collision Enabled: false 137 | Enabled: true 138 | Links: 139 | All Links Enabled: true 140 | Expand Joint Details: false 141 | Expand Link Details: false 142 | Expand Tree: false 143 | Link Tree Style: Links in Alphabetic Order 144 | base_link: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | base_link_inertia: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | bebop/imu_link: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | bebop/odometry_sensor_link: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | camera_box: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | imu_link: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | rotor_0: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | rotor_1: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | rotor_2: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | rotor_3: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | Value: true 189 | Name: RobotModel 190 | Robot Description: robot_description 191 | TF Prefix: "" 192 | Update Interval: 0 193 | Value: true 194 | Visual Enabled: true 195 | Enabled: true 196 | Global Options: 197 | Background Color: 48; 48; 48 198 | Default Light: true 199 | Fixed Frame: world 200 | Frame Rate: 30 201 | Name: root 202 | Tools: 203 | - Class: rviz/Interact 204 | Hide Inactive Objects: true 205 | - Class: rviz/MoveCamera 206 | - Class: rviz/Select 207 | - Class: rviz/FocusCamera 208 | - Class: rviz/Measure 209 | - Class: rviz/SetInitialPose 210 | Theta std deviation: 0.2617993950843811 211 | Topic: /initialpose 212 | X std deviation: 0.5 213 | Y std deviation: 0.5 214 | - Class: rviz/SetGoal 215 | Topic: /move_base_simple/goal 216 | - Class: rviz/PublishPoint 217 | Single click: true 218 | Topic: /clicked_point 219 | Value: true 220 | Views: 221 | Current: 222 | Class: rviz/Orbit 223 | Distance: 2.6823983192443848 224 | Enable Stereo Rendering: 225 | Stereo Eye Separation: 0.05999999865889549 226 | Stereo Focal Distance: 1 227 | Swap Stereo Eyes: false 228 | Value: false 229 | Focal Point: 230 | X: 0 231 | Y: 0 232 | Z: 0 233 | Focal Shape Fixed Size: false 234 | Focal Shape Size: 0.05000000074505806 235 | Invert Z Axis: false 236 | Name: Current View 237 | Near Clip Distance: 0.009999999776482582 238 | Pitch: 0.7853981852531433 239 | Target Frame: bebop/base_link 240 | Value: Orbit (rviz) 241 | Yaw: 0.7853981852531433 242 | Saved: ~ 243 | Window Geometry: 244 | Displays: 245 | collapsed: false 246 | Height: 846 247 | Hide Left Dock: false 248 | Hide Right Dock: false 249 | Image: 250 | collapsed: false 251 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae0000003efc0100000002fb0000000800540069006d00650100000000000004ae000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023d000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 252 | Selection: 253 | collapsed: false 254 | Time: 255 | collapsed: false 256 | Tool Properties: 257 | collapsed: false 258 | Views: 259 | collapsed: false 260 | Width: 1198 261 | X: 635 262 | Y: 27 263 | -------------------------------------------------------------------------------- /src/library/waypoint_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "bebop_simulator/waypoint_filter.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | 28 | using namespace std; 29 | 30 | namespace bebop_simulator { 31 | 32 | WaypointFilter::WaypointFilter() 33 | :filter_initialized_(false){ 34 | 35 | command_trajectory_private_.setFromYaw(0); 36 | command_trajectory_toSend_.setFromYaw(0); 37 | 38 | command_trajectory_private_.position_W[0] = 0; 39 | command_trajectory_private_.position_W[1] = 0; 40 | command_trajectory_private_.position_W[2] = 0; 41 | 42 | command_trajectory_toSend_.position_W[0] = 0; 43 | command_trajectory_toSend_.position_W[1] = 0; 44 | command_trajectory_toSend_.position_W[2] = 0; 45 | 46 | Tsf_ = 0; 47 | H_ = 0; 48 | 49 | 50 | } 51 | 52 | WaypointFilter::~WaypointFilter() {} 53 | 54 | // Set the filter parameters 55 | void WaypointFilter::SetParameters(WaypointFilterParameters *waypointFilter_parameters_){ 56 | 57 | 58 | Tsf_ = waypointFilter_parameters_->tsf_; 59 | H_ = waypointFilter_parameters_->h_; 60 | 61 | } 62 | 63 | // Set the trajectory point 64 | void WaypointFilter::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory_positionControllerNode){ 65 | 66 | command_trajectory_private_ = command_trajectory_positionControllerNode; 67 | 68 | } 69 | 70 | // Get the trajectory point from the control library 71 | void WaypointFilter::GetTrajectoryPoint(mav_msgs::EigenTrajectoryPoint* command_trajectory_positionController){ 72 | 73 | *command_trajectory_positionController = command_trajectory_toSend_; 74 | 75 | } 76 | 77 | // Filter initialization 78 | void WaypointFilter::Initialize(state_t state_){ 79 | 80 | if(!filter_initialized_){ 81 | 82 | command_trajectory_toSend_.position_W[0] = state_.position.x; 83 | command_trajectory_toSend_.position_W[1] = state_.position.y; 84 | command_trajectory_toSend_.position_W[2] = state_.position.z; 85 | 86 | filter_initialized_ = true; 87 | 88 | } 89 | } 90 | 91 | // Trajectory generation 92 | void WaypointFilter::TrajectoryGeneration(){ 93 | 94 | command_trajectory_toSend_.position_W[0] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[0] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[0]; 95 | command_trajectory_toSend_.position_W[1] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[1] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[1]; 96 | command_trajectory_toSend_.position_W[2] = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.position_W[2] + (H_/(Tsf_+H_)) * command_trajectory_private_.position_W[2]; 97 | 98 | double yaw = (Tsf_/(Tsf_+H_)) * command_trajectory_toSend_.getYaw() + (H_/(Tsf_+H_)) * command_trajectory_private_.getYaw(); 99 | command_trajectory_toSend_.setFromYaw(yaw); 100 | } 101 | 102 | 103 | } 104 | -------------------------------------------------------------------------------- /src/nodes/hovering_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | int main(int argc, char** argv){ 30 | ros::init(argc, argv, "hovering_example"); 31 | ros::NodeHandle nh; 32 | ros::Publisher trajectory_pub = 33 | nh.advertise( 34 | mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); 35 | ROS_INFO("Started hovering example."); 36 | 37 | std_srvs::Empty srv; 38 | bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); 39 | unsigned int i = 0; 40 | 41 | // Trying to unpause Gazebo for 10 seconds. 42 | while (i <= 10 && !unpaused) { 43 | ROS_INFO("Wait for 1 second before trying to unpause Gazebo again."); 44 | std::this_thread::sleep_for(std::chrono::seconds(1)); 45 | unpaused = ros::service::call("/gazebo/unpause_physics", srv); 46 | ++i; 47 | } 48 | 49 | if (!unpaused) { 50 | ROS_FATAL("Could not wake up Gazebo."); 51 | return -1; 52 | } 53 | else { 54 | ROS_INFO("Unpaused the Gazebo simulation."); 55 | } 56 | 57 | // Wait for t seconds to let the Gazebo GUI show up. 58 | double t = 0.5; 59 | ros::Duration(t).sleep(); 60 | 61 | trajectory_msgs::MultiDOFJointTrajectory trajectory_msg; 62 | trajectory_msg.header.stamp = ros::Time::now(); 63 | Eigen::Vector3d desired_position(0.0, 0.0, 1.0); 64 | double desired_yaw = 0.0; 65 | mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, 66 | desired_yaw, &trajectory_msg); 67 | 68 | ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].", 69 | nh.getNamespace().c_str(), 70 | desired_position.x(), 71 | desired_position.y(), 72 | desired_position.z()); 73 | trajectory_pub.publish(trajectory_msg); 74 | 75 | ros::spin(); 76 | } 77 | -------------------------------------------------------------------------------- /src/nodes/position_controller_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 20 | #define BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "bebop_simulator/common.h" 36 | #include "bebop_simulator/position_controller.h" 37 | #include "bebop_simulator/parameters_ros.h" 38 | #include "bebop_simulator/parameters.h" 39 | 40 | namespace bebop_simulator { 41 | 42 | class PositionControllerNode{ 43 | public: 44 | PositionControllerNode(); 45 | ~PositionControllerNode(); 46 | 47 | void InitializeParams(); 48 | void Publish(); 49 | 50 | private: 51 | 52 | bool waypointHasBeenPublished_ = false; 53 | 54 | PositionController position_controller_; 55 | 56 | std::string namespace_; 57 | 58 | //subscribers 59 | ros::Subscriber cmd_multi_dof_joint_trajectory_sub_; 60 | ros::Subscriber odometry_sub_; 61 | ros::Subscriber odometry_sub_gt_; 62 | 63 | //publisher 64 | ros::Publisher motor_velocity_reference_pub_; 65 | ros::Publisher odometry_filtered_pub_; 66 | ros::Publisher filtered_errors_pub_; 67 | ros::Publisher reference_angles_pub_; 68 | ros::Publisher smoothed_reference_pub_; 69 | ros::Publisher uTerr_components_pub_; 70 | ros::Publisher zVelocity_components_pub_; 71 | ros::Publisher positionAndVelocityErrors_pub_; 72 | ros::Publisher angularAndAngularVelocityErrors_pub_; 73 | 74 | nav_msgs::Odometry odometry_gt_; 75 | 76 | void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); 77 | void OdometryGTCallback(const nav_msgs::OdometryConstPtr& odometry_msg_gt); 78 | void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); 79 | 80 | 81 | }; 82 | } 83 | 84 | #endif // BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 85 | -------------------------------------------------------------------------------- /src/nodes/position_controller_with_bebop_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 20 | #define BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "bebop_simulator/common.h" 35 | #include "bebop_simulator/position_controller_with_bebop.h" 36 | #include "bebop_simulator/parameters_ros.h" 37 | #include "bebop_simulator/parameters.h" 38 | 39 | namespace bebop_simulator { 40 | 41 | class PositionControllerWithBebopNode{ 42 | public: 43 | PositionControllerWithBebopNode(); 44 | ~PositionControllerWithBebopNode(); 45 | 46 | void InitializeParams(); 47 | void Publish(); 48 | 49 | private: 50 | 51 | bool waypointHasBeenPublished_ = false; 52 | bool takeOffMsgHasBeenSent_ = false; 53 | 54 | PositionControllerWithBebop position_controller_; 55 | 56 | //subscribers 57 | ros::Subscriber cmd_multi_dof_joint_trajectory_sub_; 58 | ros::Subscriber odom_sub_; 59 | 60 | //publisher 61 | ros::Publisher motor_velocity_reference_pub_; 62 | ros::Publisher takeoff_pub_; 63 | ros::Publisher odometry_filtered_pub_; 64 | ros::Publisher reference_angles_pub_; 65 | ros::Publisher smoothed_reference_pub_; 66 | 67 | mav_msgs::EigenTrajectoryPointDeque commands_; 68 | std::deque command_waiting_times_; 69 | ros::Timer command_timer_; 70 | 71 | void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); 72 | void TakeOff(); 73 | void OdomCallback(const nav_msgs::OdometryConstPtr& odom_msg); 74 | 75 | 76 | }; 77 | } 78 | 79 | #endif // BEBOP_CONTROL_POSITION_CONTROLLER_NODE_H 80 | -------------------------------------------------------------------------------- /src/nodes/quaternion_to_rpy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include "bebop_simulator/transform_datatypes.h" 20 | #include "bebop_simulator/Matrix3x3.h" 21 | #include "bebop_simulator/Quaternion.h" 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #define M_PI 3.14159265358979323846 /* pi */ 38 | 39 | ros::Publisher rpy_publisher; 40 | ros::Subscriber quat_subscriber; 41 | 42 | void MsgCallback(const nav_msgs::Odometry odometry_msg) 43 | { 44 | // the incoming geometry_msgs::PoseStamped is transformed to a tf::Quaterion 45 | mav_msgs::EigenOdometry odometry; 46 | eigenOdometryFromMsg(odometry_msg, &odometry); 47 | tf::Quaternion q(odometry.orientation_W_B.x(), odometry.orientation_W_B.y(), odometry.orientation_W_B.z(), 48 | odometry.orientation_W_B.w()); 49 | tf::Matrix3x3 m(q); 50 | 51 | // the tf::Quaternion has a method to access roll pitch and yaw 52 | double roll, pitch, yaw; 53 | m.getRPY(roll, pitch, yaw); 54 | 55 | double yaw_degrees = yaw * 180.0 / M_PI; // conversion to degrees 56 | if( yaw_degrees < 0 ) yaw_degrees += 360.0; // convert negative to positive angles 57 | 58 | double roll_degrees = roll * 180.0 / M_PI; // conversion to degrees 59 | if( roll_degrees < 0 ) roll_degrees += 360.0; // convert negative to positive angles 60 | 61 | double pitch_degrees = pitch * 180.0 / M_PI; // conversion to degrees 62 | if( pitch_degrees < 0 ) pitch_degrees += 360.0; // convert negative to positive angles 63 | 64 | // the found angles are written in a geometry_msgs::Vector3 65 | geometry_msgs::Vector3 rpy; 66 | rpy.x = roll_degrees; 67 | rpy.y = pitch_degrees; 68 | rpy.z = yaw_degrees; 69 | 70 | // this Vector is then published: 71 | rpy_publisher.publish(rpy); 72 | ROS_DEBUG("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z); 73 | } 74 | 75 | int main(int argc, char **argv){ 76 | 77 | ros::init(argc, argv, "quaternion_to_rpy"); 78 | 79 | ros::NodeHandle n; 80 | 81 | rpy_publisher = n.advertise("orientation_rpy", 1); 82 | 83 | quat_subscriber = n.subscribe(mav_msgs::default_topics::ODOMETRY, 1, MsgCallback); 84 | 85 | // check for incoming quaternions until ctrl+c is pressed 86 | ROS_DEBUG("waiting for quaternion"); 87 | 88 | ros::spin(); 89 | 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /src/nodes/waypoint_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy 3 | * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy 4 | * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | class WaypointWithTime { 33 | public: 34 | WaypointWithTime() 35 | : waiting_time(0), 36 | position(0, 0, 0), 37 | yaw(0) { 38 | } 39 | 40 | WaypointWithTime(double t, float x, float y, float z, float _yaw) 41 | : waiting_time(t), 42 | position(x, y, z), 43 | yaw(_yaw) { 44 | } 45 | 46 | double waiting_time; 47 | Eigen::Vector3d position; 48 | double yaw; 49 | 50 | }; 51 | 52 | int main(int argc, char** argv){ 53 | ros::init(argc, argv, "waypoint_example"); 54 | ros::NodeHandle nh; 55 | ros::Publisher trajectory_pub = 56 | nh.advertise( 57 | mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); 58 | ROS_INFO("Started waypoint example."); 59 | 60 | ros::V_string args; 61 | ros::removeROSArgs(argc, argv, args); 62 | 63 | if (args.size() != 2 && args.size() != 3) { 64 | ROS_ERROR("Usage: waypoint_example " 65 | "\nThe waypoint file should be structured as: space separated: wait_time [s] x[m] y[m] z[m] yaw[deg])"); 66 | return -1; 67 | } 68 | 69 | std::vector waypoints; 70 | const float DEG_2_RAD = M_PI / 180.0; 71 | 72 | std::ifstream wp_file(args.at(1).c_str()); 73 | 74 | if (wp_file.is_open()) { 75 | double t, x, y, z, yaw; 76 | // Only read complete waypoints. 77 | while (wp_file >> t >> x >> y >> z >> yaw) { 78 | waypoints.push_back(WaypointWithTime(t, x, y, z, yaw * DEG_2_RAD)); 79 | } 80 | wp_file.close(); 81 | ROS_INFO("Read %d waypoints.", (int )waypoints.size()); 82 | } 83 | 84 | else { 85 | ROS_ERROR_STREAM("Unable to open poses file: " << args.at(1)); 86 | return -1; 87 | } 88 | 89 | std_srvs::Empty srv; 90 | bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); 91 | unsigned int i = 0; 92 | 93 | // Trying to unpause Gazebo for 10 seconds. 94 | while (i <= 10 && !unpaused) { 95 | ROS_INFO("Wait for 1 second before trying to unpause Gazebo again."); 96 | std::this_thread::sleep_for(std::chrono::seconds(1)); 97 | unpaused = ros::service::call("/gazebo/unpause_physics", srv); 98 | ++i; 99 | } 100 | 101 | if (!unpaused) { 102 | ROS_FATAL("Could not wake up Gazebo."); 103 | return -1; 104 | } 105 | else { 106 | ROS_INFO("Unpaused the Gazebo simulation."); 107 | } 108 | 109 | // Wait for t seconds to let the Gazebo GUI show up. 110 | double t = 0.5; 111 | ros::Duration(t).sleep(); 112 | 113 | ROS_INFO("Start publishing waypoints."); 114 | 115 | for (size_t i = 0; i < waypoints.size(); ++i) { 116 | 117 | trajectory_msgs::MultiDOFJointTrajectory trajectory_msg; 118 | trajectory_msg.header.stamp = ros::Time::now(); 119 | Eigen::Vector3d desired_position(waypoints[i].position.x(), waypoints[i].position.y(), waypoints[i].position.z()); 120 | double desired_yaw = waypoints[i].yaw; 121 | mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg); 122 | 123 | ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].", 124 | nh.getNamespace().c_str(), 125 | desired_position.x(), 126 | desired_position.y(), 127 | desired_position.z()); 128 | trajectory_pub.publish(trajectory_msg); 129 | 130 | // Wait for t seconds to let the Gazebo GUI show up. 131 | double t = waypoints[i].waiting_time; 132 | ros::Duration(t).sleep(); 133 | 134 | } 135 | 136 | ros::spin(); 137 | } 138 | -------------------------------------------------------------------------------- /urdf/bebop.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | $(arg namespace)/ 151 | 0 0 0 152 | base_link 153 | base_link 154 | 1 1 -0.3 155 | 0 156 | 0.0 157 | $(arg wind_direction_x) $(arg wind_direction_y) $(arg wind_direction_z) 158 | $(arg wind_start) 159 | $(arg wind_duration) 160 | $(arg wind_force) 161 | 0 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 0 0 0 0 0 0 185 | true 186 | 200 187 | 188 | 189 | 190 | 1 191 | 1 192 | 0 193 | 0 194 | 195 | 196 | 197 | 0.10 198 | 100.0 199 | 0.01 200 | 201 | 202 | gaussian 203 | 207 | 0.0 208 | 0.01 209 | 210 | 211 | 212 | laser/scan 213 | laser1D_link 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | -------------------------------------------------------------------------------- /urdf/bebop_base.urdf.xacro: -------------------------------------------------------------------------------- 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 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 49 | 50 | 51 | 52 | 53 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 0.001 151 | 152 | 153 | 154 | 155 | 156 | 161 | 162 | 163 | 164 | 169 | 170 | 171 | 172 | 173 | 174 | -------------------------------------------------------------------------------- /urdf/multirotor_base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | $(arg namespace) 58 | base_link 59 | ${rotor_velocity_slowdown_sim} 60 | 61 | 62 | 63 | 64 | 65 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | rotor_${motor_number}_joint 102 | rotor_${motor_number} 103 | ${direction} 104 | ${time_constant_up} 105 | ${time_constant_down} 106 | ${max_rot_velocity} 107 | ${motor_constant} 108 | ${moment_constant} 109 | ${robot_namespace}/command/motors 110 | ${motor_number} 111 | ${rotor_drag_coefficient} 112 | ${rolling_moment_coefficient} 113 | ${robot_namespace}/motor_vel/${motor_number} 114 | ${rotor_velocity_slowdown_sim} 115 | 116 | 117 | 118 | 119 | Gazebo/${color} 120 | 121 | 122 | 123 | 124 | 133 | 134 | 135 | 136 | 137 | 138 | 144 | 145 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 30.0 172 | 173 | 174 | 1.3962634 175 | 176 | 1920 177 | 1080 178 | R8G8B8 179 | 180 | 181 | 0.02 182 | 300 183 | 184 | 185 | gaussian 186 | 0.0 187 | 0.007 188 | 189 | 190 | 191 | 192 | true 193 | 0.0 194 | ${namespace}/camera1 195 | image_raw 196 | camera_info 197 | camera_box 198 | 0.07 199 | 0.0 200 | 0.0 201 | 0.0 202 | 0.0 203 | 0.0 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | -------------------------------------------------------------------------------- /urdf/wind_turbine.base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 52 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | transmission_interface/SimpleTransmission 92 | 93 | hardware_interface/EffortJointInterface 94 | 95 | 96 | hardware_interface/EffortJointInterface 97 | 1 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /urdf/wind_turbine.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 10 | 11 | /wind_turbine 12 | /wind_turbine/robot_description 13 | gazebo_ros_control/DefaultRobotHWSim 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /worlds/basic.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | model://sun 10 | 11 | 12 | 13 | 14 | false 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | EARTH_WGS84 26 | 47.3667 27 | 8.5500 28 | 500.0 29 | 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | quick 38 | 10 39 | 1.3 40 | 41 | 42 | 0 43 | 0.2 44 | 1000 45 | 0.001 46 | 47 | 48 | 0.001 49 | 1 50 | 1000 51 | 6.0e-06 2.3e-05 -4.2e-05 52 | 0 0 -9.8 53 | 54 | 55 | 56 | 57 | 58 | --------------------------------------------------------------------------------