├── .github ├── dependabot.yml └── workflows │ └── build.yml ├── .gitignore ├── .vscode └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── px4_ros_com │ └── frame_transforms.h ├── launch ├── offboard_control_launch.yaml └── sensor_combined_listener.launch.py ├── package.xml ├── px4_ros_com ├── __init__.py └── module_to_import.py ├── scripts ├── __init__.py ├── build_all.bash ├── build_ros2_workspace.bash └── setup_system.bash ├── src ├── examples │ ├── advertisers │ │ └── debug_vect_advertiser.cpp │ ├── listeners │ │ ├── sensor_combined_listener.cpp │ │ └── vehicle_gps_position_listener.cpp │ ├── offboard │ │ ├── offboard_control.cpp │ │ └── offboard_control_srv.cpp │ └── offboard_py │ │ └── offboard_control.py └── lib │ └── frame_transforms.cpp └── test ├── __init__.py ├── pipeline_io_test.py ├── test_input.py └── test_output.py /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: github-actions 4 | directory: "/" 5 | schedule: 6 | interval: "daily" 7 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build and Test package 2 | 3 | # CI runs over all branches 4 | on: 5 | push: 6 | branches: 7 | - 'main' 8 | 9 | defaults: 10 | run: 11 | shell: bash 12 | 13 | jobs: 14 | build_and_test: 15 | name: "ROS 2 ${{ matrix.ros2_distro }}" 16 | runs-on: ubuntu-20.04 17 | container: px4io/px4-dev-ros2-${{ matrix.ros2_distro }}:2021-05-31 18 | strategy: 19 | matrix: 20 | ros2_distro: [foxy, galactic, humble, rolling] 21 | steps: 22 | - uses: actions/checkout@v4 23 | - name: Configure workspace 24 | run: | 25 | unset ROS_DISTRO 26 | mkdir -p ~/colcon_ws/src 27 | cd ~/colcon_ws 28 | ln -s ${GITHUB_WORKSPACE} src/px4_ros_com 29 | git clone https://github.com/PX4/px4_msgs.git -b main src/px4_msgs 30 | - name: Build package 31 | run: | 32 | cd ~/colcon_ws/src/px4_ros_com/scripts 33 | ./build_ros2_workspace.bash --verbose -ros_distro ${{ matrix.ros2_distro }} --ros_path /opt/ros/${{ matrix.ros2_distro }}/setup.bash 34 | # - name: Build PX4 Firmware 35 | # run: | 36 | # git clone https://github.com/PX4/Firmware.git ~/PX4/Firmware 37 | # cd ~/PX4/Firmware 38 | # DONT_RUN=1 make px4_sitl_rtps gazebo 39 | # - name: SITL integration test - data output 40 | # run: | 41 | # source ~/colcon_ws/install/setup.bash 42 | # cd ~/colcon_ws/src/px4_ros_com/test 43 | # python3 pipeline_io_test.py -f ~/PX4/Firmware/ -p debug_vect -t fcu_output 44 | # - name: SITL integration test - data input 45 | # run: | 46 | # source ~/colcon_ws/install/setup.bash 47 | # cd ~/colcon_ws/src/px4_ros_com/test 48 | # python3 pipeline_io_test.py -f ~/PX4/Firmware/ -s sensor_combined -t fcu_input 49 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | scripts/__pycache__ 3 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "chrono": "cpp", 4 | "ostream": "cpp" 5 | } 6 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(px4_ros_com) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(builtin_interfaces REQUIRED) 21 | find_package(eigen3_cmake_module REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | find_package(geometry_msgs REQUIRED) 24 | find_package(px4_msgs REQUIRED) 25 | find_package(rclcpp REQUIRED) 26 | find_package(sensor_msgs REQUIRED) 27 | find_package(rclpy REQUIRED) 28 | 29 | ################# 30 | # Setup targets # 31 | ################# 32 | 33 | include_directories(include) 34 | 35 | # Add frame_transforms lib 36 | add_library(frame_transforms SHARED src/lib/frame_transforms.cpp) 37 | ament_target_dependencies(frame_transforms Eigen3 geometry_msgs sensor_msgs) 38 | target_include_directories(frame_transforms PUBLIC 39 | $ 40 | $ 41 | ) 42 | 43 | 44 | # examples/listeners/sensor_combined_listener 45 | add_executable(sensor_combined_listener src/examples/listeners/sensor_combined_listener.cpp) 46 | ament_target_dependencies(sensor_combined_listener rclcpp px4_msgs) 47 | install(TARGETS sensor_combined_listener DESTINATION lib/${PROJECT_NAME}) 48 | 49 | # examples/listeners/vehicle_gps_position_listener 50 | add_executable(vehicle_gps_position_listener src/examples/listeners/vehicle_gps_position_listener.cpp) 51 | ament_target_dependencies(vehicle_gps_position_listener rclcpp px4_msgs) 52 | install(TARGETS vehicle_gps_position_listener DESTINATION lib/${PROJECT_NAME}) 53 | 54 | # examples/advertisers/debug_vect_advertiser 55 | add_executable(debug_vect_advertiser src/examples/advertisers/debug_vect_advertiser.cpp) 56 | ament_target_dependencies(debug_vect_advertiser rclcpp px4_msgs) 57 | install(TARGETS debug_vect_advertiser DESTINATION lib/${PROJECT_NAME}) 58 | 59 | # examples/offboard/offboard_control 60 | add_executable(offboard_control src/examples/offboard/offboard_control.cpp) 61 | ament_target_dependencies(offboard_control rclcpp px4_msgs) 62 | install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME}) 63 | 64 | # examples/offboard/offboard_control_srv 65 | add_executable(offboard_control_srv src/examples/offboard/offboard_control_srv.cpp) 66 | ament_target_dependencies(offboard_control_srv rclcpp px4_msgs) 67 | install(TARGETS offboard_control_srv DESTINATION lib/${PROJECT_NAME}) 68 | 69 | 70 | ############ 71 | # Install ## 72 | ############ 73 | 74 | # Export information to downstream packages 75 | ament_export_dependencies(ament_cmake rclcpp rosidl_default_runtime eigen3_cmake_module Eigen3 px4_msgs geometry_msgs sensor_msgs) 76 | 77 | ament_export_targets(export_frame_transforms HAS_LIBRARY_TARGET) 78 | 79 | ament_export_include_directories(include) 80 | ament_export_libraries(frame_transforms) 81 | 82 | # Install header files 83 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) 84 | 85 | install(TARGETS frame_transforms 86 | EXPORT export_frame_transforms 87 | ARCHIVE DESTINATION lib 88 | LIBRARY DESTINATION lib 89 | RUNTIME DESTINATION bin 90 | INCLUDES DESTINATION include 91 | ) 92 | 93 | # Install launch files. 94 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) 95 | 96 | # Install tests 97 | install(DIRECTORY test DESTINATION share/${PROJECT_NAME}/) 98 | 99 | 100 | ############ 101 | # Testing ## 102 | ############ 103 | 104 | if(BUILD_TESTING) 105 | find_package(ament_lint_auto REQUIRED) 106 | # the following line skips the linter which checks for copyrights 107 | # uncomment the line when a copyright and license is not present in all source files 108 | #set(ament_cmake_copyright_FOUND TRUE) 109 | # the following line skips cpplint (only works in a git repo) 110 | # uncomment the line when this package is not in a git repo 111 | #set(ament_cmake_cpplint_FOUND TRUE) 112 | ament_lint_auto_find_test_dependencies() 113 | endif() 114 | 115 | ########### 116 | # Python ## 117 | ########### 118 | 119 | # Install Python modules 120 | ament_python_install_package(${PROJECT_NAME}) 121 | 122 | # Install Python executables 123 | install(PROGRAMS 124 | src/examples/offboard_py/offboard_control.py 125 | DESTINATION lib/${PROJECT_NAME} 126 | ) 127 | 128 | ament_package() 129 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018-2019, PX4 Development Team 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PX4-ROS2 bridge 2 | 3 | [![GitHub license](https://img.shields.io/github/license/PX4/px4_ros_com.svg)](https://github.com/PX4/px4_ros_com/blob/master/LICENSE) [![GitHub (pre-)release](https://img.shields.io/github/release-pre/PX4/px4_ros_com.svg)](https://github.com/PX4/px4_ros_com/releases/tag/beta) [![DOI](https://zenodo.org/badge/142936318.svg)](https://zenodo.org/badge/latestdoi/142936318) [![Build and Test package](https://github.com/PX4/px4_ros_com/workflows/Build%20and%20Test%20package/badge.svg?branch=master)](https://github.com/PX4/px4_ros_com/actions) 4 | 5 | [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) 6 | 7 | This package provides example nodes for exchanging data and commands between ROS2 and PX4. 8 | It also provides a [library](./include/px4_ros_com/frame_transforms.h) to ease the conversion between ROS2 and PX4 frame conventions. 9 | It has a straight dependency on the [`px4_msgs`](https://github.com/PX4/px4_msgs) package. 10 | 11 | ## Install, build and usage 12 | 13 | Check the [uXRCE-DDS](https://docs.px4.io/main/en/middleware/uxrce_dds.html) and the [ROS2 Interface](https://docs.px4.io/main/en/ros/ros2_comm.html) sections on the PX4 Devguide for details on how to install the required dependencies, build the package and use it. 14 | 15 | ## Bug tracking and feature requests 16 | 17 | Use the [Issues](https://github.com/PX4/px4_ros_com/issues) section to create a new issue. Report your issue or feature request [here](https://github.com/PX4/px4_ros_com/issues/new). 18 | 19 | ## Questions and troubleshooting 20 | 21 | Reach the PX4 development team on the [PX4 Discord Server](https://discord.gg/dronecode). 22 | -------------------------------------------------------------------------------- /include/px4_ros_com/frame_transforms.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2020 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @brief Functions to use on frame transforms 35 | * @file frame_transform.h 36 | * @addtogroup lib 37 | * @author Nuno Marques 38 | * 39 | * Adapted from MAVROS frame_tf.h 40 | */ 41 | 42 | #ifndef FRAME_TRANSFORMS_H 43 | #define FRAME_TRANSFORMS_H 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | // for Covariance types 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | namespace px4_ros_com 57 | { 58 | namespace frame_transforms 59 | { 60 | 61 | //! Type matching rosmsg for 3x3 covariance matrix 62 | using Covariance3d = sensor_msgs::msg::Imu::_angular_velocity_covariance_type; 63 | 64 | //! Type matching rosmsg for 6x6 covariance matrix 65 | using Covariance6d = geometry_msgs::msg::PoseWithCovariance::_covariance_type; 66 | 67 | //! Type matching rosmsg for 9x9 covariance matrix 68 | using Covariance9d = std::array; 69 | 70 | //! Eigen::Map for Covariance3d 71 | using EigenMapCovariance3d = Eigen::Map>; 72 | using EigenMapConstCovariance3d = Eigen::Map>; 73 | 74 | //! Eigen::Map for Covariance6d 75 | using EigenMapCovariance6d = Eigen::Map>; 76 | using EigenMapConstCovariance6d = Eigen::Map>; 77 | 78 | //! Eigen::Map for Covariance9d 79 | using EigenMapCovariance9d = Eigen::Map>; 80 | using EigenMapConstCovariance9d = Eigen::Map>; 81 | 82 | /** 83 | * @brief Orientation transform options when applying rotations to data 84 | */ 85 | enum class StaticTF { 86 | NED_TO_ENU, //!< change from expressed WRT NED frame to WRT ENU frame 87 | ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame 88 | AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame 89 | BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft 90 | ECEF_TO_ENU, //!< change from expressed WRT ECEF frame to WRT ENU frame 91 | ENU_TO_ECEF //!< change from expressed WRT ENU frame to WRT ECEF frame 92 | }; 93 | 94 | // Utils to ease conversions 95 | namespace utils 96 | { 97 | 98 | // Quaternion 99 | namespace quaternion 100 | { 101 | 102 | /** 103 | * @brief Convert euler angles to quaternion. 104 | */ 105 | Eigen::Quaterniond quaternion_from_euler(const Eigen::Vector3d &euler); 106 | 107 | /** 108 | * @brief Convert euler angles to quaternion. 109 | * 110 | * @return quaternion, same as @a tf::quaternionFromeuler() but in Eigen format. 111 | */ 112 | Eigen::Quaterniond quaternion_from_euler(const double roll, const double pitch, const double yaw); 113 | 114 | /** 115 | * @brief Convert quaternion to euler angles 116 | * @return Eigen::Quaterniond 117 | */ 118 | Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q); 119 | 120 | /** 121 | * @brief Convert quaternion to euler angles 122 | */ 123 | void quaternion_to_euler(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw); 124 | 125 | /** 126 | * @brief Store Quaternion to float[4] 127 | * Eigen::Quaterniond xyzw internal order to PX4 quaternion wxyz. 128 | */ 129 | void eigen_quat_to_array(const Eigen::Quaterniond &q, std::array &qarray); 130 | 131 | /** 132 | * @brief Convert float[4] quaternion to Eigen Quaternion 133 | */ 134 | Eigen::Quaterniond array_to_eigen_quat(const std::array &q); 135 | 136 | /** 137 | * @brief Get Yaw angle from quaternion 138 | */ 139 | double quaternion_get_yaw(const Eigen::Quaterniond &q); 140 | 141 | } // namespace quaternion 142 | 143 | // Data types 144 | namespace types 145 | { 146 | 147 | /** 148 | * @brief Convert covariance matrix to float[n] 149 | */ 150 | template void covariance_to_array(const T &cov, std::array &covmsg); 151 | 152 | /** 153 | * @brief Convert upper right triangular of a covariance matrix to float[n] array 154 | */ 155 | template 156 | void covariance_urt_to_array(const T &covmap, std::array &covmsg); 157 | 158 | /** 159 | * @brief Convert float[n] array (upper right triangular of a covariance matrix) 160 | * to Eigen::MatrixXd full covariance matrix 161 | */ 162 | template 163 | void array_urt_to_covariance_matrix(const std::array &covmsg, T &covmat); 164 | 165 | } // namespace types 166 | } // namespace utils 167 | 168 | /** 169 | * @brief Static quaternion needed for rotating between ENU and NED frames 170 | * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) 171 | * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) 172 | */ 173 | static const auto NED_ENU_Q = utils::quaternion::quaternion_from_euler(M_PI, 0.0, M_PI_2); 174 | 175 | /** 176 | * @brief Static quaternion needed for rotating between aircraft and base_link frames 177 | * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) 178 | * Fto Forward, Left, Up (base_link) frames. 179 | */ 180 | static const auto AIRCRAFT_BASELINK_Q = utils::quaternion::quaternion_from_euler(M_PI, 0.0, 0.0); 181 | 182 | /** 183 | * @brief Static vector needed for rotating between ENU and NED frames 184 | * +PI rotation around X (North) axis follwed by +PI/2 rotation about Z (Down) 185 | * gives the ENU frame. Similarly, a +PI rotation about X (East) followed by 186 | * a +PI/2 roation about Z (Up) gives the NED frame. 187 | */ 188 | static const Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q); 189 | 190 | /** 191 | * @brief Static vector needed for rotating between aircraft and base_link frames 192 | * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft) 193 | * Fto Forward, Left, Up (base_link) frames. 194 | */ 195 | static const Eigen::Affine3d AIRCRAFT_BASELINK_AFFINE(AIRCRAFT_BASELINK_Q); 196 | 197 | /** 198 | * @brief 3-D matrices to fill 6-D rotation matrix applied to change covariance matrices coordinate frames 199 | */ 200 | static const auto NED_ENU_R = NED_ENU_Q.normalized().toRotationMatrix(); 201 | static const auto AIRCRAFT_BASELINK_R = AIRCRAFT_BASELINK_Q.normalized().toRotationMatrix(); 202 | 203 | /** 204 | * @brief Use reflections instead of rotations for NED <-> ENU transformation 205 | * to avoid NaN/Inf floating point pollution across different axes 206 | * since in NED <-> ENU the axes are perfectly aligned. 207 | */ 208 | static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1, 0, 2)); 209 | static const Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z(1, 1, -1); 210 | 211 | /** 212 | * @brief Auxiliar matrices to Covariance transforms 213 | */ 214 | using Matrix6d = Eigen::Matrix; 215 | using Matrix9d = Eigen::Matrix; 216 | 217 | /** 218 | * @brief Transform representation of orientation from 1 frame to another. 219 | * (e.g. transfrom orientation from representing from base_link -> NED to 220 | * representing base_link -> ENU) 221 | */ 222 | Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform); 223 | 224 | /** 225 | * @brief Transform data expressed in one frame to another. 226 | */ 227 | Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q); 228 | 229 | /** 230 | * @brief Transform 3x3 convariance expressed in one frame to another. 231 | */ 232 | Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q); 233 | 234 | /** 235 | * @brief Transform 6x6 convariance expressed in one frame to another. 236 | */ 237 | Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q); 238 | 239 | /** 240 | * @brief Transform 9x9 convariance expressed in one frame to another. 241 | */ 242 | Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q); 243 | 244 | /** 245 | * @brief Transform data expressed in one frame to another. 246 | */ 247 | Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform); 248 | 249 | /** 250 | * @brief Transform 3d convariance expressed in one frame to another. 251 | */ 252 | Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform); 253 | 254 | /** 255 | * @brief Transform 6d convariance expressed in one frame to another. 256 | */ 257 | Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform); 258 | 259 | /** 260 | * @brief Transform 9d convariance expressed in one frame to another 261 | */ 262 | Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform); 263 | 264 | /** 265 | * @brief Transform data expressed in one frame to another frame with additional 266 | * map origin parameter. 267 | */ 268 | Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, 269 | const StaticTF transform); 270 | 271 | /** 272 | * @brief Transform from orientation represented WRT NED frame to orientation 273 | * represented WRT ENU frame 274 | */ 275 | template inline T ned_to_enu_orientation(const T &in) 276 | { 277 | return transform_orientation(in, StaticTF::NED_TO_ENU); 278 | } 279 | 280 | /** 281 | * @brief Transform from orientation represented WRT ENU frame to orientation 282 | * represented WRT NED frame 283 | */ 284 | template inline T enu_to_ned_orientation(const T &in) 285 | { 286 | return transform_orientation(in, StaticTF::ENU_TO_NED); 287 | } 288 | 289 | /** 290 | * @brief Transform from orientation represented WRT aircraft body frame to 291 | * orientation represented WRT base_link body frame 292 | */ 293 | template inline T aircraft_to_baselink_orientation(const T &in) 294 | { 295 | return transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK); 296 | } 297 | 298 | /** 299 | * @brief Transform from orientation represented WRT base_link body frame to 300 | * orientation represented WRT aircraft body frame 301 | */ 302 | template inline T baselink_to_aircraft_orientation(const T &in) 303 | { 304 | return transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); 305 | } 306 | 307 | /** 308 | * @brief Transform from orientation represented in PX4 format to ROS format 309 | * PX4 format is aircraft to NED 310 | * ROS format is baselink to ENU 311 | * 312 | * Two steps conversion: 313 | * 1. aircraft to NED is converted to aircraft to ENU (NED_to_ENU conversion) 314 | * 2. aircraft to ENU is converted to baselink to ENU (baselink_to_aircraft conversion) 315 | */ 316 | template inline T px4_to_ros_orientation(const T &in) 317 | { 318 | return baselink_to_aircraft_orientation(ned_to_enu_orientation(in)); 319 | } 320 | 321 | /** 322 | * @brief Transform from orientation represented in ROS format to PX4 format 323 | * PX4 format is aircraft to NED 324 | * ROS format is baselink to ENU 325 | * 326 | * Two steps conversion: 327 | * 1. baselink to ENU is converted to baselink to NED (ENU_to_NED conversion) 328 | * 2. baselink to NED is converted to aircraft to NED (aircraft_to_baselink conversion) 329 | */ 330 | template inline T ros_to_px4_orientation(const T &in) 331 | { 332 | return aircraft_to_baselink_orientation(enu_to_ned_orientation(in)); 333 | } 334 | 335 | /** 336 | * @brief Transform data expressed in NED to ENU local frame. 337 | */ 338 | template inline T ned_to_enu_local_frame(const T &in) 339 | { 340 | return transform_static_frame(in, StaticTF::NED_TO_ENU); 341 | } 342 | 343 | /** 344 | * @brief Transform data expressed in ENU to NED frame. 345 | */ 346 | template inline T enu_to_ned_local_frame(const T &in) 347 | { 348 | return transform_static_frame(in, StaticTF::ENU_TO_NED); 349 | } 350 | 351 | /** 352 | * @brief Transform data expressed in aircraft body frame to base_link body frame. 353 | */ 354 | template inline T aircraft_to_baselink_body_frame(const T &in) 355 | { 356 | return transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK); 357 | } 358 | 359 | /** 360 | * @brief Transform data expressed in base_link body frame to aircraft body frame. 361 | */ 362 | template inline T baselink_to_aircraft_body_frame(const T &in) 363 | { 364 | return transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT); 365 | } 366 | 367 | /** 368 | * @brief Transform data expressed in ECEF frame to ENU frame. 369 | * 370 | * @param in local ECEF coordinates [m] 371 | * @param map_origin geodetic origin [lla] 372 | * @returns local ENU coordinates [m]. 373 | */ 374 | template inline T ecef_to_enu_local_frame(const T &in, const T &map_origin) 375 | { 376 | return transform_static_frame(in, map_origin, StaticTF::ECEF_TO_ENU); 377 | } 378 | 379 | /** 380 | * @brief Transform data expressed in ENU frame to ECEF frame. 381 | * 382 | * @param in local ENU coordinates [m] 383 | * @param map_origin geodetic origin [lla] 384 | * @returns local ECEF coordinates [m]. 385 | */ 386 | template inline T enu_to_ecef_local_frame(const T &in, const T &map_origin) 387 | { 388 | return transform_static_frame(in, map_origin, StaticTF::ENU_TO_ECEF); 389 | } 390 | 391 | /** 392 | * @brief Transform data expressed in aircraft frame to NED frame. 393 | * Assumes quaternion represents rotation from aircraft frame to NED frame. 394 | */ 395 | template inline T aircraft_to_ned_frame(const T &in, const Eigen::Quaterniond &q) 396 | { 397 | return transform_frame(in, q); 398 | } 399 | 400 | /** 401 | * @brief Transform data expressed in NED to aircraft frame. 402 | * Assumes quaternion represents rotation from NED to aircraft frame. 403 | */ 404 | template inline T ned_to_aircraft_frame(const T &in, const Eigen::Quaterniond &q) 405 | { 406 | return transform_frame(in, q); 407 | } 408 | 409 | /** 410 | * @brief Transform data expressed in aircraft frame to ENU frame. 411 | * Assumes quaternion represents rotation from aircraft frame to ENU frame. 412 | */ 413 | template inline T aircraft_to_enu_frame(const T &in, const Eigen::Quaterniond &q) 414 | { 415 | return transform_frame(in, q); 416 | } 417 | 418 | /** 419 | * @brief Transform data expressed in ENU to aircraft frame. 420 | * Assumes quaternion represents rotation from ENU to aircraft frame. 421 | */ 422 | template inline T enu_to_aircraft_frame(const T &in, const Eigen::Quaterniond &q) 423 | { 424 | return transform_frame(in, q); 425 | } 426 | 427 | /** 428 | * @brief Transform data expressed in baselink to ENU frame. 429 | * Assumes quaternion represents rotation from basel_link to ENU frame. 430 | */ 431 | template inline T baselink_to_enu_frame(const T &in, const Eigen::Quaterniond &q) 432 | { 433 | return transform_frame(in, q); 434 | } 435 | 436 | /** 437 | * @brief Transform data expressed in ENU to base_link frame. 438 | * Assumes quaternion represents rotation from ENU to base_link frame. 439 | */ 440 | template inline T enu_to_baselink_frame(const T &in, const Eigen::Quaterniond &q) 441 | { 442 | return transform_frame(in, q); 443 | } 444 | 445 | } // namespace frame_transforms 446 | } // namespace px4_ros_com 447 | 448 | #endif // FRAME_TRANSFORMS_H 449 | -------------------------------------------------------------------------------- /launch/offboard_control_launch.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | 3 | - node: 4 | pkg: "px4_ros_com" 5 | exec: "offboard_control" 6 | name: "offboard_control" 7 | output: "screen" -------------------------------------------------------------------------------- /launch/sensor_combined_listener.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ################################################################################ 4 | # 5 | # Copyright (c) 2018-2022, PX4 Development Team. All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # 2. Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # 3. Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software without 19 | # specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ################################################################################ 34 | 35 | """ 36 | Example to launch a sensor_combined listener node. 37 | """ 38 | 39 | from launch import LaunchDescription 40 | from launch_ros.actions import Node 41 | from launch.actions import ExecuteProcess 42 | 43 | def generate_launch_description(): 44 | 45 | micro_ros_agent = ExecuteProcess( 46 | cmd=[[ 47 | 'micro-ros-agent udp4 --port 8888 -v ' 48 | ]], 49 | shell=True 50 | ) 51 | 52 | sensor_combined_listener_node = Node( 53 | package='px4_ros_com', 54 | executable='sensor_combined_listener', 55 | output='screen', 56 | shell=True, 57 | ) 58 | 59 | return LaunchDescription([ 60 | #micro_ros_agent, 61 | sensor_combined_listener_node 62 | ]) 63 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | px4_ros_com 5 | 0.1.0 6 | Package to interface ROS 2 with PX4 7 | 8 | Beniamino Pozzan 9 | Beniamino Pozzan 10 | Nuno Marques 11 | 12 | BSD 3-Clause 13 | 14 | ament_cmake 15 | eigen3_cmake_module 16 | 17 | eigen 18 | ros_environment 19 | 20 | builtin_interfaces 21 | rclcpp 22 | 23 | px4_msgs 24 | geometry_msgs 25 | sensor_msgs 26 | 27 | launch 28 | launch_testing 29 | launch_testing_ros 30 | 31 | rosidl_default_runtime 32 | ros2launch 33 | 34 | eigen3_cmake_module 35 | eigen 36 | 37 | ament_lint_auto 38 | ament_lint_common 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /px4_ros_com/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/px4_ros_com/86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af/px4_ros_com/__init__.py -------------------------------------------------------------------------------- /px4_ros_com/module_to_import.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/px4_ros_com/86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af/px4_ros_com/module_to_import.py -------------------------------------------------------------------------------- /scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/px4_ros_com/86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af/scripts/__init__.py -------------------------------------------------------------------------------- /scripts/build_all.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # parse help argument 5 | if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then 6 | echo -e "Usage: build_all.bash [option...] \t This script builds px4_ros_com workspaces for ROS 2 and ROS (1)" >&2 7 | echo 8 | echo -e "\t--ros2_distro \t\t Set ROS 2 distro name (dashing|eloquent|foxy|galactic|humble|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" 9 | echo -e "\t--ros2_path \t\t Set ROS 2 environment setup.bash location. Useful for source installs. If not set, the script sources the environment in /opt/ros/$ROS_DISTRO/" 10 | echo -e "\t--verbose \t\t Add more verbosity to the console output" 11 | echo 12 | exit 0 13 | fi 14 | 15 | SCRIPT_DIR=$0 16 | if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then 17 | SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) 18 | fi 19 | 20 | # parse the arguments 21 | while [ $# -gt 0 ]; do 22 | if [[ $1 == *"--"* ]]; then 23 | v="${1/--/}" 24 | if [ ! -z $2 ]; then 25 | declare $v="$2" 26 | else 27 | declare $v=1 28 | fi 29 | fi 30 | shift 31 | done 32 | 33 | unset ROS_DISTRO 34 | # One can pass the ROS_DISTRO's using the '--ros2_distro' args 35 | if [ -z $ros2_distro]; then 36 | # set the ROS_DISTRO variables automatically based on the Ubuntu codename and 37 | # ROS install directory 38 | # The distros are order by priority (according to being LTS vs non-LTS) 39 | case "$(lsb_release -s -c)" in "bionic") 40 | 41 | if [ -d "/opt/ros/foxy" ]; then 42 | export ROS2_DISTRO="foxy" 43 | elif [ -d "/opt/ros/galactic" ]; then 44 | export ROS2_DISTRO="galactic" 45 | elif [ -d "/opt/ros/humble" ]; then 46 | export ROS2_DISTRO="humble" 47 | 48 | else 49 | if [ -z $ros2_path ]; then 50 | echo "- No ROS 2 distro installed or not installed in the default directory." 51 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/eloquent/install). Otherwise, please install ROS 2 Dashing following https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Binary/" 52 | exit 1 53 | else 54 | # source the ROS2 environment (from arg) 55 | source $ros2_path 56 | export ROS2_DISTRO="$(rosversion -d)" 57 | fi 58 | fi 59 | ;; 60 | "focal") 61 | 62 | if [ -d "/opt/ros/foxy" ]; then 63 | export ROS2_DISTRO="foxy" 64 | elif [ -d "/opt/ros/galactic" ]; then 65 | ROS_DISTRO="galactic" 66 | elif [ -d "/opt/ros/rolling" ]; then 67 | ROS_DISTRO="rolling" 68 | else 69 | if [ -z $ros2_path ]; then 70 | echo "- No ROS 2 distro installed or not installed in the default directory." 71 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Foxy following https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Binary/" 72 | exit 1 73 | else 74 | # source the ROS2 environment (from arg) 75 | source $ros2_path 76 | export ROS2_DISTRO="$(rosversion -d)" 77 | fi 78 | fi 79 | ;; 80 | "jammy") 81 | 82 | if [ -d "/opt/ros/humble" ]; then 83 | ROS2_DISTRO="humble" 84 | elif [ -d "/opt/ros/rolling" ]; then 85 | ROS2_DISTRO="rolling" 86 | else 87 | if [ -z $ros2_path ]; then 88 | echo "- No ROS 2 distro installed or not installed in the default directory." 89 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros2_path' arg! (ex: ~/ros_src/humble/install). Otherwise, please install ROS 2 Humble following https://docs.ros.org/en/humble/Installation.html" 90 | exit 1 91 | else 92 | # source the ROS2 environment (from arg) 93 | source $ros2_path 94 | export ROS2_DISTRO="$(rosversion -d)" 95 | fi 96 | fi 97 | ;; 98 | *) 99 | echo "Unsupported version of Ubuntu detected." 100 | exit 1 101 | ;; 102 | esac 103 | else 104 | 105 | if [ -z $ros2_path ]; then 106 | echo "- Warning: You set a ROS 2 manually to be used." 107 | echo " This assumes you want to use another ROS 2 version installed on your system. Please set the install location with '--ros2_path' arg! (ex: --ros_path ~/ros_src/eloquent/install/setup.bash)" 108 | exit 1 109 | else 110 | export ROS2_DISTRO="$ros2_distro" 111 | fi 112 | fi 113 | 114 | # setup the required path variables 115 | export ROS2_REPO_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) 116 | export ROS2_WS_SRC_DIR=$(cd "$(dirname "$ROS2_REPO_DIR")" && pwd) 117 | export ROS2_WS_DIR=$(cd "$(dirname "$ROS2_WS_SRC_DIR")" && pwd) 118 | 119 | # Check if gnome-terminal exists 120 | if [ -x "$(command -v gnome-terminal)" ]; then 121 | SHELL_TERM="gnome-terminal --tab -- /bin/bash -c" 122 | echo $SHELL_TERM 123 | # Check if xterm exists 124 | elif [ -x "$(command -v xterm)" ]; then 125 | SHELL_TERM="xterm -e" 126 | echo $SHELL_TERM 127 | fi 128 | 129 | printf "\n************* Building ROS2 workspace *************\n\n" 130 | 131 | # source the ROS2 environment 132 | if [ -z $ros2_path ]; then 133 | source /opt/ros/$ROS2_DISTRO/setup.bash 134 | else 135 | source $ros2_path 136 | fi 137 | 138 | # build px4_ros_com package 139 | [ ! -v $verbose ] && colcon_output=$(echo "--event-handlers console_direct+") 140 | cd $ROS2_WS_DIR && colcon build $colcon_output 141 | 142 | $SHELL_TERM \ 143 | ''' 144 | # source the ROS2 workspace 145 | unset ROS_DISTRO && source $ROS2_WS_DIR/install/setup.bash 146 | 147 | # source the ROS2 workspace environment so to have it ready to use 148 | unset ROS_DISTRO && source $ROS2_WS_DIR/install/setup.bash 149 | 150 | exec /bin/bash 151 | ''' & 152 | 153 | # source the ROS2 workspace environment so to have it ready to use 154 | source $ROS2_WS_DIR/install/setup.bash 155 | 156 | printf "\nROS2 workspace ready...\n\n" 157 | 158 | $SHELL_TERM \ 159 | ''' 160 | # source the ROS2 workspace environment so to have it ready to use 161 | source $ROS2_WS_DIR/install/setup.bash 162 | 163 | printf "To start the XRCE-DDS agent, use \"micro-ros-agent udp4 --port 2019\"\n\n" 164 | exec /bin/bash 165 | ''' & 166 | -------------------------------------------------------------------------------- /scripts/build_ros2_workspace.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # parse help argument 5 | if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then 6 | echo -e "Usage: build_ros2_workspace.bash [option...] \t This script builds px4_ros_com workspace for ROS 2" >&2 7 | echo 8 | echo -e "\t--ros_distro \t\t Set ROS 2 distro name (dashing|eloquent|foxy|galactic|humble|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" 9 | echo -e "\t--ros_path \t\t Set ROS 2 environment setup.bash location. Useful for source installs. If not set, the script sources the environment in /opt/ros/$ROS_DISTRO" 10 | echo -e "\t--verbose \t\t Add more verbosity to the console output" 11 | echo 12 | exit 0 13 | fi 14 | 15 | SCRIPT_DIR=$0 16 | if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then 17 | SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) 18 | fi 19 | 20 | # parse the arguments 21 | while [ $# -gt 0 ]; do 22 | if [[ $1 == *"--"* ]]; then 23 | v="${1/--/}" 24 | if [ ! -z $2 ]; then 25 | declare $v="$2" 26 | else 27 | declare $v=1 28 | fi 29 | fi 30 | shift 31 | done 32 | 33 | # One can pass the ROS_DISTRO using the '--ros_distro' arg 34 | unset ROS_DISTRO 35 | if [ -z $ros_distro ]; then 36 | # set the ROS_DISTRO variables automatically based on the Ubuntu codename and 37 | # ROS 2 install directory 38 | # The distros are order by priority (according to being LTS vs non-LTS) 39 | case "$(lsb_release -s -c)" in 40 | "bionic") 41 | if [ -d "/opt/ros/dashing" ]; then 42 | ROS_DISTRO="dashing" 43 | elif [ -d "/opt/ros/eloquent" ]; then 44 | ROS_DISTRO="eloquent" 45 | else 46 | if [ -z $ros_path ]; then 47 | echo "- No ROS 2 distro installed or not installed in the default directory." 48 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/eloquent/install). Otherwise, please install ROS 2 Dashing following https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Binary/" 49 | exit 1 50 | else 51 | # source the ROS2 environment (from arg) 52 | source $ros_path 53 | fi 54 | fi 55 | ;; 56 | "focal") 57 | if [ -d "/opt/ros/foxy" ]; then 58 | ROS_DISTRO="foxy" 59 | elif [ -d "/opt/ros/galactic" ]; then 60 | ROS_DISTRO="galactic" 61 | elif [ -d "/opt/ros/rolling" ]; then 62 | ROS_DISTRO="rolling" 63 | else 64 | if [ -z $ros_path ]; then 65 | echo "- No ROS 2 distro installed or not installed in the default directory." 66 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Foxy following https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Binary/" 67 | exit 1 68 | else 69 | # source the ROS2 environment (from arg) 70 | source $ros_path 71 | fi 72 | fi 73 | ;; 74 | "jammy") 75 | if [ -d "/opt/ros/humble" ]; then 76 | ROS_DISTRO="humble" 77 | elif [ -d "/opt/ros/rolling" ]; then 78 | ROS_DISTRO="rolling" 79 | else 80 | if [ -z $ros_path ]; then 81 | echo "- No ROS 2 distro installed or not installed in the default directory." 82 | echo " If you are using a ROS 2 version installed from source, please set the install location with '--ros_path' arg! (ex: ~/ros_src/foxy/install). Otherwise, please install ROS 2 Humble following https://docs.ros.org/en/humble/Installation.html" 83 | exit 1 84 | else 85 | # source the ROS2 environment (from arg) 86 | source $ros_path 87 | fi 88 | fi 89 | ;; 90 | *) 91 | echo "Unsupported version of Ubuntu detected." 92 | exit 1 93 | ;; 94 | esac 95 | # source the ROS2 environment 96 | source /opt/ros/$ROS_DISTRO/setup.bash 97 | else 98 | if [ -z $ros_path ]; then 99 | echo "- Warning: You set a ROS 2 manually to be used." 100 | echo " This assumes you want to use another ROS 2 version installed on your system. Please set the install location with '--ros_path' arg! (ex: --ros_path ~/ros_src/eloquent/install/setup.bash)" 101 | exit 1 102 | else 103 | # source the ROS 2 environment (from arg) 104 | source $ros_path 105 | fi 106 | fi 107 | 108 | # setup the required path variables 109 | ROS_REPO_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) 110 | ROS_WS_SRC_DIR=$(cd "$(dirname "$ROS_REPO_DIR")" && pwd) 111 | ROS_WS_DIR=$(cd "$(dirname "$ROS_WS_SRC_DIR")" && pwd) 112 | 113 | # build px4_ros_com package 114 | [ ! -v $verbose ] && colcon_output=$(echo "--event-handlers console_direct+") 115 | cd $ROS_WS_DIR && colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELWITHDEBINFO --symlink-install $colcon_output 116 | 117 | # source the ROS2 workspace environment so to have it ready to use 118 | source $ROS_WS_DIR/install/setup.bash 119 | 120 | printf "\nROS2 workspace ready...\n\n" 121 | -------------------------------------------------------------------------------- /scripts/setup_system.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # parse help argument 5 | if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then 6 | echo -e "Usage: setup_system.bash [option...] \t This script setups the system with the required dependencies." >&2 7 | echo 8 | echo -e "\t--ros2_distro \t Set ROS2 distro name (dashing|eloquent|foxy|galactic|rolling). If not set, the script will set the ROS_DISTRO env variable based on the Ubuntu codename" 9 | echo -e "\t--clean \t If set, issues 'apt-get autoremove', 'apt-get autoclean' and deletes temp files." 10 | echo 11 | exit 0 12 | fi 13 | 14 | # parse the arguments 15 | while [ $# -gt 0 ]; do 16 | if [[ $1 == *"--"* ]]; then 17 | v="${1/--/}" 18 | if [ ! -z $2 ]; then 19 | declare $v="$2" 20 | else 21 | declare $v=1 22 | fi 23 | fi 24 | shift 25 | done 26 | 27 | # One can pass the ROS_DISTRO's using the '--ros2_distro' args 28 | unset ROS_DISTRO 29 | if [ -z $ros2_distro ]; then 30 | # set the ROS_DISTRO variables automatically based on the Ubuntu codename. 31 | # the following distros are the recommended as they are LTS 32 | case "$(lsb_release -s -c)" in 33 | "bionic") 34 | ROS2_DISTRO="dashing" 35 | ;; 36 | "focal") 37 | ROS2_DISTRO="foxy" 38 | ;; 39 | *) 40 | echo "Unsupported version of Ubuntu detected." 41 | exit 1 42 | ;; 43 | esac 44 | else 45 | ROS2_DISTRO="$ros2_distro" 46 | fi 47 | 48 | # Install ROS2 dependencies 49 | sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 50 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \ 51 | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 52 | 53 | echo "Updating package lists ..." 54 | sudo apt-get -qq update 55 | sudo apt-get -qq dist-upgrade 56 | echo "Installing ROS2 $ROS2_DISTRO and some dependencies..." 57 | 58 | sudo apt-get install -y \ 59 | dirmngr \ 60 | gnupg2 \ 61 | python3-colcon-common-extensions \ 62 | python3-dev \ 63 | ros-$ROS2_DISTRO-desktop \ 64 | ros-$ROS2_DISTRO-eigen3-cmake-module \ 65 | ros-$ROS2_DISTRO-launch-testing-ament-cmake \ 66 | ros-$ROS2_DISTRO-rosidl-generator-dds-idl 67 | 68 | # Install Python3 packages needed for testing 69 | curl https://bootstrap.pypa.io/get-pip.py | python3 && 70 | python3 -m pip install --upgrade pip \ 71 | setuptools \ 72 | argcomplete \ 73 | flake8 \ 74 | flake8-blind-except \ 75 | flake8-builtins \ 76 | flake8-class-newline \ 77 | flake8-comprehensions \ 78 | flake8-deprecated \ 79 | flake8-docstrings \ 80 | flake8-import-order \ 81 | flake8-quotes \ 82 | pytest \ 83 | pytest-cov \ 84 | pytest-repeat \ 85 | pytest-runner \ 86 | pytest-rerunfailures 87 | 88 | # Install Python3 packages for uORB topic generation 89 | python3 -c "import em" || python3 -m pip install --user empy 90 | python3 -c "import genmsg.template_tools" || python3 -m pip install --user pyros-genmsg 91 | python3 -c "import packaging" || python3 -m pip install --user packaging 92 | 93 | # Clean residuals 94 | if [ -o clean ]; then 95 | sudo apt-get -y autoremove && 96 | sudo apt-get clean autoclean && 97 | sudo rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/* 98 | fi 99 | -------------------------------------------------------------------------------- /src/examples/advertisers/debug_vect_advertiser.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @brief Debug Vect uORB topic adverstiser example 35 | * @file debug_vect_advertiser.cpp 36 | * @addtogroup examples 37 | * @author Nuno Marques 38 | */ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | using namespace std::chrono_literals; 45 | 46 | class DebugVectAdvertiser : public rclcpp::Node 47 | { 48 | public: 49 | DebugVectAdvertiser() : Node("debug_vect_advertiser") 50 | { 51 | 52 | publisher_ = this->create_publisher("/fmu/in/debug_vect", 10); 53 | 54 | auto timer_callback = [this]()->void { 55 | auto debug_vect = px4_msgs::msg::DebugVect(); 56 | debug_vect.timestamp = std::chrono::time_point_cast(std::chrono::steady_clock::now()).time_since_epoch().count(); 57 | std::string name = "test"; 58 | std::copy(name.begin(), name.end(), debug_vect.name.begin()); 59 | debug_vect.x = 1.0; 60 | debug_vect.y = 2.0; 61 | debug_vect.z = 3.0; 62 | RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %lu x: %f y: %f z: %f \033[0m", 63 | debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z); 64 | 65 | this->publisher_->publish(debug_vect); 66 | }; 67 | 68 | timer_ = this->create_wall_timer(500ms, timer_callback); 69 | } 70 | 71 | private: 72 | rclcpp::TimerBase::SharedPtr timer_; 73 | rclcpp::Publisher::SharedPtr publisher_; 74 | }; 75 | 76 | int main(int argc, char *argv[]) 77 | { 78 | std::cout << "Starting debug_vect advertiser node..." << std::endl; 79 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 80 | rclcpp::init(argc, argv); 81 | rclcpp::spin(std::make_shared()); 82 | 83 | rclcpp::shutdown(); 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /src/examples/listeners/sensor_combined_listener.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). 4 | * 2018 PX4 Pro Development Team. All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, this 10 | * list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @brief Sensor Combined uORB topic listener example 36 | * @file sensor_combined_listener.cpp 37 | * @addtogroup examples 38 | * @author Nuno Marques 39 | * @author Vicente Monge 40 | */ 41 | 42 | #include 43 | #include 44 | 45 | /** 46 | * @brief Sensor Combined uORB topic data callback 47 | */ 48 | class SensorCombinedListener : public rclcpp::Node 49 | { 50 | public: 51 | explicit SensorCombinedListener() : Node("sensor_combined_listener") 52 | { 53 | rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; 54 | auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); 55 | 56 | subscription_ = this->create_subscription("/fmu/out/sensor_combined", qos, 57 | [this](const px4_msgs::msg::SensorCombined::UniquePtr msg) { 58 | std::cout << "\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n"; 59 | std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl; 60 | std::cout << "=============================" << std::endl; 61 | std::cout << "ts: " << msg->timestamp << std::endl; 62 | std::cout << "gyro_rad[0]: " << msg->gyro_rad[0] << std::endl; 63 | std::cout << "gyro_rad[1]: " << msg->gyro_rad[1] << std::endl; 64 | std::cout << "gyro_rad[2]: " << msg->gyro_rad[2] << std::endl; 65 | std::cout << "gyro_integral_dt: " << msg->gyro_integral_dt << std::endl; 66 | std::cout << "accelerometer_timestamp_relative: " << msg->accelerometer_timestamp_relative << std::endl; 67 | std::cout << "accelerometer_m_s2[0]: " << msg->accelerometer_m_s2[0] << std::endl; 68 | std::cout << "accelerometer_m_s2[1]: " << msg->accelerometer_m_s2[1] << std::endl; 69 | std::cout << "accelerometer_m_s2[2]: " << msg->accelerometer_m_s2[2] << std::endl; 70 | std::cout << "accelerometer_integral_dt: " << msg->accelerometer_integral_dt << std::endl; 71 | }); 72 | } 73 | 74 | private: 75 | rclcpp::Subscription::SharedPtr subscription_; 76 | 77 | }; 78 | 79 | int main(int argc, char *argv[]) 80 | { 81 | std::cout << "Starting sensor_combined listener node..." << std::endl; 82 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 83 | rclcpp::init(argc, argv); 84 | rclcpp::spin(std::make_shared()); 85 | 86 | rclcpp::shutdown(); 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /src/examples/listeners/vehicle_gps_position_listener.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2019 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @brief Vehicle GPS position uORB topic listener example 35 | * @file vehicle_global_position_listener.cpp 36 | * @addtogroup examples 37 | * @author Nuno Marques 38 | */ 39 | 40 | #include 41 | #include 42 | 43 | /** 44 | * @brief Vehicle GPS position uORB topic data callback 45 | */ 46 | class VehicleGpsPositionListener : public rclcpp::Node 47 | { 48 | public: 49 | explicit VehicleGpsPositionListener() : Node("vehicle_global_position_listener") 50 | { 51 | rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; 52 | auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); 53 | 54 | subscription_ = this->create_subscription("/fmu/out/vehicle_gps_position", qos, 55 | [this](const px4_msgs::msg::SensorGps::UniquePtr msg) { 56 | std::cout << "\n\n\n\n\n\n\n\n\n\n"; 57 | std::cout << "RECEIVED VEHICLE GPS POSITION DATA" << std::endl; 58 | std::cout << "==================================" << std::endl; 59 | std::cout << "ts: " << msg->timestamp << std::endl; 60 | std::cout << "lat: " << msg->latitude_deg << std::endl; 61 | std::cout << "lon: " << msg->longitude_deg << std::endl; 62 | std::cout << "alt: " << msg->altitude_msl_m << std::endl; 63 | std::cout << "alt_ellipsoid: " << msg->altitude_ellipsoid_m << std::endl; 64 | std::cout << "s_variance_m_s: " << msg->s_variance_m_s << std::endl; 65 | std::cout << "c_variance_rad: " << msg->c_variance_rad << std::endl; 66 | std::cout << "fix_type: " << msg->fix_type << std::endl; 67 | std::cout << "eph: " << msg->eph << std::endl; 68 | std::cout << "epv: " << msg->epv << std::endl; 69 | std::cout << "hdop: " << msg->hdop << std::endl; 70 | std::cout << "vdop: " << msg->vdop << std::endl; 71 | std::cout << "noise_per_ms: " << msg->noise_per_ms << std::endl; 72 | std::cout << "vel_m_s: " << msg->vel_m_s << std::endl; 73 | std::cout << "vel_n_m_s: " << msg->vel_n_m_s << std::endl; 74 | std::cout << "vel_e_m_s: " << msg->vel_e_m_s << std::endl; 75 | std::cout << "vel_d_m_s: " << msg->vel_d_m_s << std::endl; 76 | std::cout << "cog_rad: " << msg->cog_rad << std::endl; 77 | std::cout << "vel_ned_valid: " << msg->vel_ned_valid << std::endl; 78 | std::cout << "timestamp_time_relative: " << msg->timestamp_time_relative << std::endl; 79 | std::cout << "time_utc_usec: " << msg->time_utc_usec << std::endl; 80 | std::cout << "satellites_used: " << msg->satellites_used << std::endl; 81 | std::cout << "heading: " << msg->heading << std::endl; 82 | std::cout << "heading_offset: " << msg->heading_offset << std::endl; 83 | }); 84 | } 85 | 86 | private: 87 | rclcpp::Subscription::SharedPtr subscription_; 88 | }; 89 | 90 | int main(int argc, char *argv[]) 91 | { 92 | std::cout << "Starting vehicle_global_position listener node..." << std::endl; 93 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 94 | rclcpp::init(argc, argv); 95 | rclcpp::spin(std::make_shared()); 96 | 97 | rclcpp::shutdown(); 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /src/examples/offboard/offboard_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2020 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @brief Offboard control example 35 | * @file offboard_control.cpp 36 | * @addtogroup examples 37 | * @author Mickey Cowden 38 | * @author Nuno Marques 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | 51 | using namespace std::chrono; 52 | using namespace std::chrono_literals; 53 | using namespace px4_msgs::msg; 54 | 55 | class OffboardControl : public rclcpp::Node 56 | { 57 | public: 58 | OffboardControl() : Node("offboard_control") 59 | { 60 | 61 | offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); 62 | trajectory_setpoint_publisher_ = this->create_publisher("/fmu/in/trajectory_setpoint", 10); 63 | vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); 64 | 65 | offboard_setpoint_counter_ = 0; 66 | 67 | auto timer_callback = [this]() -> void { 68 | 69 | if (offboard_setpoint_counter_ == 10) { 70 | // Change to Offboard mode after 10 setpoints 71 | this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); 72 | 73 | // Arm the vehicle 74 | this->arm(); 75 | } 76 | 77 | // offboard_control_mode needs to be paired with trajectory_setpoint 78 | publish_offboard_control_mode(); 79 | publish_trajectory_setpoint(); 80 | 81 | // stop the counter after reaching 11 82 | if (offboard_setpoint_counter_ < 11) { 83 | offboard_setpoint_counter_++; 84 | } 85 | }; 86 | timer_ = this->create_wall_timer(100ms, timer_callback); 87 | } 88 | 89 | void arm(); 90 | void disarm(); 91 | 92 | private: 93 | rclcpp::TimerBase::SharedPtr timer_; 94 | 95 | rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; 96 | rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; 97 | rclcpp::Publisher::SharedPtr vehicle_command_publisher_; 98 | 99 | std::atomic timestamp_; //!< common synced timestamped 100 | 101 | uint64_t offboard_setpoint_counter_; //!< counter for the number of setpoints sent 102 | 103 | void publish_offboard_control_mode(); 104 | void publish_trajectory_setpoint(); 105 | void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); 106 | }; 107 | 108 | /** 109 | * @brief Send a command to Arm the vehicle 110 | */ 111 | void OffboardControl::arm() 112 | { 113 | publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); 114 | 115 | RCLCPP_INFO(this->get_logger(), "Arm command send"); 116 | } 117 | 118 | /** 119 | * @brief Send a command to Disarm the vehicle 120 | */ 121 | void OffboardControl::disarm() 122 | { 123 | publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); 124 | 125 | RCLCPP_INFO(this->get_logger(), "Disarm command send"); 126 | } 127 | 128 | /** 129 | * @brief Publish the offboard control mode. 130 | * For this example, only position and altitude controls are active. 131 | */ 132 | void OffboardControl::publish_offboard_control_mode() 133 | { 134 | OffboardControlMode msg{}; 135 | msg.position = true; 136 | msg.velocity = false; 137 | msg.acceleration = false; 138 | msg.attitude = false; 139 | msg.body_rate = false; 140 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 141 | offboard_control_mode_publisher_->publish(msg); 142 | } 143 | 144 | /** 145 | * @brief Publish a trajectory setpoint 146 | * For this example, it sends a trajectory setpoint to make the 147 | * vehicle hover at 5 meters with a yaw angle of 180 degrees. 148 | */ 149 | void OffboardControl::publish_trajectory_setpoint() 150 | { 151 | TrajectorySetpoint msg{}; 152 | msg.position = {0.0, 0.0, -5.0}; 153 | msg.yaw = -3.14; // [-PI:PI] 154 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 155 | trajectory_setpoint_publisher_->publish(msg); 156 | } 157 | 158 | /** 159 | * @brief Publish vehicle commands 160 | * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) 161 | * @param param1 Command parameter 1 162 | * @param param2 Command parameter 2 163 | */ 164 | void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2) 165 | { 166 | VehicleCommand msg{}; 167 | msg.param1 = param1; 168 | msg.param2 = param2; 169 | msg.command = command; 170 | msg.target_system = 1; 171 | msg.target_component = 1; 172 | msg.source_system = 1; 173 | msg.source_component = 1; 174 | msg.from_external = true; 175 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 176 | vehicle_command_publisher_->publish(msg); 177 | } 178 | 179 | int main(int argc, char *argv[]) 180 | { 181 | std::cout << "Starting offboard control node..." << std::endl; 182 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 183 | rclcpp::init(argc, argv); 184 | rclcpp::spin(std::make_shared()); 185 | 186 | rclcpp::shutdown(); 187 | return 0; 188 | } 189 | -------------------------------------------------------------------------------- /src/examples/offboard/offboard_control_srv.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2023 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @brief Offboard control example 35 | * @file offboard_control.cpp 36 | * @addtogroup examples * 37 | * @author Beniamino Pozzan 38 | * @author Mickey Cowden 39 | * @author Nuno Marques 40 | */ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | using namespace std::chrono; 54 | using namespace std::chrono_literals; 55 | using namespace px4_msgs::msg; 56 | 57 | class OffboardControl : public rclcpp::Node 58 | { 59 | public: 60 | OffboardControl(std::string px4_namespace) : 61 | Node("offboard_control_srv"), 62 | state_{State::init}, 63 | service_result_{0}, 64 | service_done_{false}, 65 | offboard_control_mode_publisher_{this->create_publisher(px4_namespace+"in/offboard_control_mode", 10)}, 66 | trajectory_setpoint_publisher_{this->create_publisher(px4_namespace+"in/trajectory_setpoint", 10)}, 67 | vehicle_command_client_{this->create_client(px4_namespace+"vehicle_command")} 68 | { 69 | RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services"); 70 | RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service"); 71 | while (!vehicle_command_client_->wait_for_service(1s)) { 72 | if (!rclcpp::ok()) { 73 | RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); 74 | return; 75 | } 76 | RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); 77 | } 78 | 79 | timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this)); 80 | } 81 | 82 | void switch_to_offboard_mode(); 83 | void arm(); 84 | void disarm(); 85 | 86 | private: 87 | enum class State{ 88 | init, 89 | offboard_requested, 90 | wait_for_stable_offboard_mode, 91 | arm_requested, 92 | armed 93 | } state_; 94 | uint8_t service_result_; 95 | bool service_done_; 96 | rclcpp::TimerBase::SharedPtr timer_; 97 | 98 | rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; 99 | rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; 100 | rclcpp::Client::SharedPtr vehicle_command_client_; 101 | 102 | 103 | void publish_offboard_control_mode(); 104 | void publish_trajectory_setpoint(); 105 | void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); 106 | void response_callback(rclcpp::Client::SharedFuture future); 107 | void timer_callback(void); 108 | }; 109 | 110 | /** 111 | * @brief Send a command to switch to offboard mode 112 | */ 113 | void OffboardControl::switch_to_offboard_mode(){ 114 | RCLCPP_INFO(this->get_logger(), "requesting switch to Offboard mode"); 115 | request_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); 116 | } 117 | 118 | /** 119 | * @brief Send a command to Arm the vehicle 120 | */ 121 | void OffboardControl::arm() 122 | { 123 | RCLCPP_INFO(this->get_logger(), "requesting arm"); 124 | request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); 125 | } 126 | 127 | /** 128 | * @brief Send a command to Disarm the vehicle 129 | */ 130 | void OffboardControl::disarm() 131 | { 132 | RCLCPP_INFO(this->get_logger(), "requesting disarm"); 133 | request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); 134 | } 135 | 136 | /** 137 | * @brief Publish the offboard control mode. 138 | * For this example, only position and altitude controls are active. 139 | */ 140 | void OffboardControl::publish_offboard_control_mode() 141 | { 142 | OffboardControlMode msg{}; 143 | msg.position = true; 144 | msg.velocity = false; 145 | msg.acceleration = false; 146 | msg.attitude = false; 147 | msg.body_rate = false; 148 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 149 | offboard_control_mode_publisher_->publish(msg); 150 | } 151 | 152 | /** 153 | * @brief Publish a trajectory setpoint 154 | * For this example, it sends a trajectory setpoint to make the 155 | * vehicle hover at 5 meters with a yaw angle of 180 degrees. 156 | */ 157 | void OffboardControl::publish_trajectory_setpoint() 158 | { 159 | TrajectorySetpoint msg{}; 160 | msg.position = {0.0, 0.0, -5.0}; 161 | msg.yaw = -3.14; // [-PI:PI] 162 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 163 | trajectory_setpoint_publisher_->publish(msg); 164 | } 165 | 166 | /** 167 | * @brief Publish vehicle commands 168 | * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) 169 | * @param param1 Command parameter 1 170 | * @param param2 Command parameter 2 171 | */ 172 | void OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2) 173 | { 174 | auto request = std::make_shared(); 175 | 176 | VehicleCommand msg{}; 177 | msg.param1 = param1; 178 | msg.param2 = param2; 179 | msg.command = command; 180 | msg.target_system = 1; 181 | msg.target_component = 1; 182 | msg.source_system = 1; 183 | msg.source_component = 1; 184 | msg.from_external = true; 185 | msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; 186 | request->request = msg; 187 | 188 | service_done_ = false; 189 | auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this, 190 | std::placeholders::_1)); 191 | RCLCPP_INFO(this->get_logger(), "Command send"); 192 | } 193 | 194 | void OffboardControl::timer_callback(void){ 195 | static uint8_t num_of_steps = 0; 196 | 197 | // offboard_control_mode needs to be paired with trajectory_setpoint 198 | publish_offboard_control_mode(); 199 | publish_trajectory_setpoint(); 200 | 201 | switch (state_) 202 | { 203 | case State::init : 204 | switch_to_offboard_mode(); 205 | state_ = State::offboard_requested; 206 | break; 207 | case State::offboard_requested : 208 | if(service_done_){ 209 | if (service_result_==0){ 210 | RCLCPP_INFO(this->get_logger(), "Entered offboard mode"); 211 | state_ = State::wait_for_stable_offboard_mode; 212 | } 213 | else{ 214 | RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting"); 215 | rclcpp::shutdown(); 216 | } 217 | } 218 | break; 219 | case State::wait_for_stable_offboard_mode : 220 | if (++num_of_steps>10){ 221 | arm(); 222 | state_ = State::arm_requested; 223 | } 224 | break; 225 | case State::arm_requested : 226 | if(service_done_){ 227 | if (service_result_==0){ 228 | RCLCPP_INFO(this->get_logger(), "vehicle is armed"); 229 | state_ = State::armed; 230 | } 231 | else{ 232 | RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting"); 233 | rclcpp::shutdown(); 234 | } 235 | } 236 | break; 237 | default: 238 | break; 239 | } 240 | } 241 | 242 | void OffboardControl::response_callback( 243 | rclcpp::Client::SharedFuture future) { 244 | auto status = future.wait_for(1s); 245 | if (status == std::future_status::ready) { 246 | auto reply = future.get()->reply; 247 | service_result_ = reply.result; 248 | switch (service_result_) 249 | { 250 | case reply.VEHICLE_CMD_RESULT_ACCEPTED: 251 | RCLCPP_INFO(this->get_logger(), "command accepted"); 252 | break; 253 | case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: 254 | RCLCPP_WARN(this->get_logger(), "command temporarily rejected"); 255 | break; 256 | case reply.VEHICLE_CMD_RESULT_DENIED: 257 | RCLCPP_WARN(this->get_logger(), "command denied"); 258 | break; 259 | case reply.VEHICLE_CMD_RESULT_UNSUPPORTED: 260 | RCLCPP_WARN(this->get_logger(), "command unsupported"); 261 | break; 262 | case reply.VEHICLE_CMD_RESULT_FAILED: 263 | RCLCPP_WARN(this->get_logger(), "command failed"); 264 | break; 265 | case reply.VEHICLE_CMD_RESULT_IN_PROGRESS: 266 | RCLCPP_WARN(this->get_logger(), "command in progress"); 267 | break; 268 | case reply.VEHICLE_CMD_RESULT_CANCELLED: 269 | RCLCPP_WARN(this->get_logger(), "command cancelled"); 270 | break; 271 | default: 272 | RCLCPP_WARN(this->get_logger(), "command reply unknown"); 273 | break; 274 | } 275 | service_done_ = true; 276 | } else { 277 | RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); 278 | } 279 | } 280 | 281 | int main(int argc, char *argv[]) 282 | { 283 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 284 | rclcpp::init(argc, argv); 285 | rclcpp::spin(std::make_shared("/fmu/")); 286 | 287 | rclcpp::shutdown(); 288 | return 0; 289 | } 290 | -------------------------------------------------------------------------------- /src/examples/offboard_py/offboard_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy 6 | from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus 7 | 8 | 9 | class OffboardControl(Node): 10 | """Node for controlling a vehicle in offboard mode.""" 11 | 12 | def __init__(self) -> None: 13 | super().__init__('offboard_control_takeoff_and_land') 14 | 15 | # Configure QoS profile for publishing and subscribing 16 | qos_profile = QoSProfile( 17 | reliability=ReliabilityPolicy.BEST_EFFORT, 18 | durability=DurabilityPolicy.TRANSIENT_LOCAL, 19 | history=HistoryPolicy.KEEP_LAST, 20 | depth=1 21 | ) 22 | 23 | # Create publishers 24 | self.offboard_control_mode_publisher = self.create_publisher( 25 | OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile) 26 | self.trajectory_setpoint_publisher = self.create_publisher( 27 | TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile) 28 | self.vehicle_command_publisher = self.create_publisher( 29 | VehicleCommand, '/fmu/in/vehicle_command', qos_profile) 30 | 31 | # Create subscribers 32 | self.vehicle_local_position_subscriber = self.create_subscription( 33 | VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile) 34 | self.vehicle_status_subscriber = self.create_subscription( 35 | VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile) 36 | 37 | # Initialize variables 38 | self.offboard_setpoint_counter = 0 39 | self.vehicle_local_position = VehicleLocalPosition() 40 | self.vehicle_status = VehicleStatus() 41 | self.takeoff_height = -5.0 42 | 43 | # Create a timer to publish control commands 44 | self.timer = self.create_timer(0.1, self.timer_callback) 45 | 46 | def vehicle_local_position_callback(self, vehicle_local_position): 47 | """Callback function for vehicle_local_position topic subscriber.""" 48 | self.vehicle_local_position = vehicle_local_position 49 | 50 | def vehicle_status_callback(self, vehicle_status): 51 | """Callback function for vehicle_status topic subscriber.""" 52 | self.vehicle_status = vehicle_status 53 | 54 | def arm(self): 55 | """Send an arm command to the vehicle.""" 56 | self.publish_vehicle_command( 57 | VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0) 58 | self.get_logger().info('Arm command sent') 59 | 60 | def disarm(self): 61 | """Send a disarm command to the vehicle.""" 62 | self.publish_vehicle_command( 63 | VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0) 64 | self.get_logger().info('Disarm command sent') 65 | 66 | def engage_offboard_mode(self): 67 | """Switch to offboard mode.""" 68 | self.publish_vehicle_command( 69 | VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0) 70 | self.get_logger().info("Switching to offboard mode") 71 | 72 | def land(self): 73 | """Switch to land mode.""" 74 | self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND) 75 | self.get_logger().info("Switching to land mode") 76 | 77 | def publish_offboard_control_heartbeat_signal(self): 78 | """Publish the offboard control mode.""" 79 | msg = OffboardControlMode() 80 | msg.position = True 81 | msg.velocity = False 82 | msg.acceleration = False 83 | msg.attitude = False 84 | msg.body_rate = False 85 | msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) 86 | self.offboard_control_mode_publisher.publish(msg) 87 | 88 | def publish_position_setpoint(self, x: float, y: float, z: float): 89 | """Publish the trajectory setpoint.""" 90 | msg = TrajectorySetpoint() 91 | msg.position = [x, y, z] 92 | msg.yaw = 1.57079 # (90 degree) 93 | msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) 94 | self.trajectory_setpoint_publisher.publish(msg) 95 | self.get_logger().info(f"Publishing position setpoints {[x, y, z]}") 96 | 97 | def publish_vehicle_command(self, command, **params) -> None: 98 | """Publish a vehicle command.""" 99 | msg = VehicleCommand() 100 | msg.command = command 101 | msg.param1 = params.get("param1", 0.0) 102 | msg.param2 = params.get("param2", 0.0) 103 | msg.param3 = params.get("param3", 0.0) 104 | msg.param4 = params.get("param4", 0.0) 105 | msg.param5 = params.get("param5", 0.0) 106 | msg.param6 = params.get("param6", 0.0) 107 | msg.param7 = params.get("param7", 0.0) 108 | msg.target_system = 1 109 | msg.target_component = 1 110 | msg.source_system = 1 111 | msg.source_component = 1 112 | msg.from_external = True 113 | msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) 114 | self.vehicle_command_publisher.publish(msg) 115 | 116 | def timer_callback(self) -> None: 117 | """Callback function for the timer.""" 118 | self.publish_offboard_control_heartbeat_signal() 119 | 120 | if self.offboard_setpoint_counter == 10: 121 | self.engage_offboard_mode() 122 | self.arm() 123 | 124 | if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD: 125 | self.publish_position_setpoint(0.0, 0.0, self.takeoff_height) 126 | 127 | elif self.vehicle_local_position.z <= self.takeoff_height: 128 | self.land() 129 | exit(0) 130 | 131 | if self.offboard_setpoint_counter < 11: 132 | self.offboard_setpoint_counter += 1 133 | 134 | 135 | def main(args=None) -> None: 136 | print('Starting offboard control node...') 137 | rclpy.init(args=args) 138 | offboard_control = OffboardControl() 139 | rclpy.spin(offboard_control) 140 | offboard_control.destroy_node() 141 | rclpy.shutdown() 142 | 143 | 144 | if __name__ == '__main__': 145 | try: 146 | main() 147 | except Exception as e: 148 | print(e) 149 | -------------------------------------------------------------------------------- /src/lib/frame_transforms.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright 2020 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | ****************************************************************************/ 32 | 33 | /** 34 | * @file frame_transform.cpp 35 | * @addtogroup lib 36 | * @author Nuno Marques 37 | * 38 | * Adapted from MAVROS ftf_frame_conversions.cpp and ftf_quaternion_utils.cpp. 39 | */ 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace px4_ros_com 46 | { 47 | namespace frame_transforms 48 | { 49 | 50 | // Utils to ease conversions 51 | namespace utils 52 | { 53 | 54 | // Quaternion 55 | namespace quaternion 56 | { 57 | 58 | Eigen::Quaterniond quaternion_from_euler(const Eigen::Vector3d &euler) 59 | { 60 | // YPR is ZYX axes 61 | return Eigen::Quaterniond(Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * 62 | Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * 63 | Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX())); 64 | } 65 | 66 | Eigen::Quaterniond quaternion_from_euler(const double roll, const double pitch, const double yaw) 67 | { 68 | return quaternion_from_euler(Eigen::Vector3d(roll, pitch, yaw)); 69 | } 70 | 71 | Eigen::Vector3d quaternion_to_euler(const Eigen::Quaterniond &q) 72 | { 73 | // YPR is ZYX axes 74 | return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); 75 | } 76 | 77 | void quaternion_to_euler(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw) 78 | { 79 | const auto euler = quaternion_to_euler(q); 80 | roll = euler.x(); 81 | pitch = euler.y(); 82 | yaw = euler.z(); 83 | } 84 | 85 | void eigen_quat_to_array(const Eigen::Quaterniond &q, std::array &qarray) 86 | { 87 | qarray[0] = q.w(); 88 | qarray[1] = q.x(); 89 | qarray[2] = q.y(); 90 | qarray[3] = q.z(); 91 | } 92 | 93 | Eigen::Quaterniond array_to_eigen_quat(const std::array &q) 94 | { 95 | return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); 96 | } 97 | 98 | double quaternion_get_yaw(const Eigen::Quaterniond &q) 99 | { 100 | const double &q0 = q.w(); 101 | const double &q1 = q.x(); 102 | const double &q2 = q.y(); 103 | const double &q3 = q.z(); 104 | 105 | return std::atan2(2. * (q0 * q3 + q1 * q2), 1. - 2. * (q2 * q2 + q3 * q3)); 106 | } 107 | 108 | } // namespace quaternion 109 | 110 | // Data types 111 | namespace types 112 | { 113 | 114 | template void covariance_to_array(const T &cov, std::array &covmsg) 115 | { 116 | std::copy(cov.cbegin(), cov.cend(), covmsg.begin()); 117 | } 118 | 119 | template 120 | void covariance_urt_to_array(const T &covmap, std::array &covmsg) 121 | { 122 | auto m = covmap; 123 | std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2; 124 | assert(COV_SIZE == ARR_SIZE && 125 | ("covariance matrix URT size (%lu) is different from uORB msg covariance field size (%lu)", COV_SIZE, 126 | ARR_SIZE)); 127 | 128 | auto out = covmsg.begin(); 129 | 130 | for (size_t x = 0; x < m.cols(); x++) { 131 | for (size_t y = x; y < m.rows(); y++) 132 | *out++ = m(y, x); 133 | } 134 | } 135 | 136 | template 137 | void array_urt_to_covariance_matrix(const std::array &covmsg, T &covmat) 138 | { 139 | std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2; 140 | assert(COV_SIZE == ARR_SIZE && 141 | ("covariance matrix URT size (%lu) is different from uORB msg covariance field size (%lu)", COV_SIZE, 142 | ARR_SIZE)); 143 | 144 | auto in = covmsg.begin(); 145 | 146 | for (size_t x = 0; x < covmat.cols(); x++) { 147 | for (size_t y = x; y < covmat.rows(); y++) { 148 | covmat(x, y) = static_cast(*in++); 149 | covmat(y, x) = covmat(x, y); 150 | } 151 | } 152 | } 153 | 154 | } // namespace types 155 | } // namespace utils 156 | 157 | Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform) 158 | { 159 | Eigen::Quaterniond out; 160 | 161 | switch (transform) { 162 | case StaticTF::NED_TO_ENU: 163 | case StaticTF::ENU_TO_NED: 164 | out = NED_ENU_Q * q; 165 | break; 166 | 167 | case StaticTF::AIRCRAFT_TO_BASELINK: 168 | case StaticTF::BASELINK_TO_AIRCRAFT: 169 | out = q * AIRCRAFT_BASELINK_Q; 170 | break; 171 | 172 | default: 173 | break; 174 | } 175 | 176 | return out; 177 | } 178 | 179 | Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform) 180 | { 181 | Eigen::Vector3d out; 182 | switch (transform) { 183 | case StaticTF::NED_TO_ENU: 184 | case StaticTF::ENU_TO_NED: 185 | out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * vec); 186 | break; 187 | 188 | case StaticTF::AIRCRAFT_TO_BASELINK: 189 | case StaticTF::BASELINK_TO_AIRCRAFT: 190 | out = AIRCRAFT_BASELINK_AFFINE * vec; 191 | break; 192 | 193 | default: 194 | break; 195 | } 196 | 197 | return out; 198 | } 199 | 200 | Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform) 201 | { 202 | Covariance3d cov_out_; 203 | EigenMapConstCovariance3d cov_in(cov.data()); 204 | EigenMapCovariance3d cov_out(cov_out_.data()); 205 | 206 | switch (transform) { 207 | case StaticTF::NED_TO_ENU: 208 | case StaticTF::ENU_TO_NED: 209 | cov_out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * cov_in * NED_ENU_REFLECTION_Z) * 210 | NED_ENU_REFLECTION_XY.transpose(); 211 | break; 212 | 213 | case StaticTF::AIRCRAFT_TO_BASELINK: 214 | case StaticTF::BASELINK_TO_AIRCRAFT: 215 | cov_out = cov_in * AIRCRAFT_BASELINK_Q; 216 | break; 217 | 218 | default: 219 | break; 220 | } 221 | 222 | return cov_out_; 223 | } 224 | 225 | Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform) 226 | { 227 | Covariance6d cov_out_; 228 | Matrix6d R = Matrix6d::Zero(); // not `auto` because Zero ret is const 229 | 230 | EigenMapConstCovariance6d cov_in(cov.data()); 231 | EigenMapCovariance6d cov_out(cov_out_.data()); 232 | 233 | switch (transform) { 234 | case StaticTF::NED_TO_ENU: 235 | case StaticTF::ENU_TO_NED: { 236 | Eigen::PermutationMatrix<6> NED_ENU_REFLECTION_XY_6(NED_ENU_REFLECTION_XY.indices().replicate<2, 1>()); 237 | NED_ENU_REFLECTION_XY_6.indices().middleRows<3>(3).array() += 3; 238 | Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_6( 239 | NED_ENU_REFLECTION_Z.diagonal().replicate<2, 1>()); 240 | 241 | cov_out = NED_ENU_REFLECTION_XY_6 * (NED_ENU_REFLECTION_Z_6 * cov_in * NED_ENU_REFLECTION_Z_6) * 242 | NED_ENU_REFLECTION_XY_6.transpose(); 243 | break; 244 | } 245 | case StaticTF::AIRCRAFT_TO_BASELINK: 246 | case StaticTF::BASELINK_TO_AIRCRAFT: 247 | R.block<3, 3>(0, 0) = 248 | R.block<3, 3>(3, 3) = 249 | AIRCRAFT_BASELINK_R; 250 | 251 | cov_out = R * cov_in * R.transpose(); 252 | break; 253 | 254 | default: 255 | break; 256 | } 257 | 258 | return cov_out_; 259 | } 260 | 261 | Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform) 262 | { 263 | Covariance9d cov_out_; 264 | Matrix9d R = Matrix9d::Zero(); 265 | 266 | EigenMapConstCovariance9d cov_in(cov.data()); 267 | EigenMapCovariance9d cov_out(cov_out_.data()); 268 | 269 | switch (transform) { 270 | case StaticTF::NED_TO_ENU: 271 | case StaticTF::ENU_TO_NED: { 272 | Eigen::PermutationMatrix<9> NED_ENU_REFLECTION_XY_9(NED_ENU_REFLECTION_XY.indices().replicate<3, 1>()); 273 | NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(3).array() += 3; 274 | NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(6).array() += 6; 275 | Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_9( 276 | NED_ENU_REFLECTION_Z.diagonal().replicate<3, 1>()); 277 | 278 | cov_out = NED_ENU_REFLECTION_XY_9 * (NED_ENU_REFLECTION_Z_9 * cov_in * NED_ENU_REFLECTION_Z_9) * 279 | NED_ENU_REFLECTION_XY_9.transpose(); 280 | 281 | break; 282 | } 283 | case StaticTF::AIRCRAFT_TO_BASELINK: 284 | case StaticTF::BASELINK_TO_AIRCRAFT: 285 | R.block<3, 3>(0, 0) = 286 | R.block<3, 3>(3, 3) = 287 | R.block<3, 3>(6, 6) = 288 | AIRCRAFT_BASELINK_R; 289 | 290 | cov_out = R * cov_in * R.transpose(); 291 | 292 | break; 293 | 294 | default: 295 | break; 296 | } 297 | 298 | return cov_out_; 299 | } 300 | 301 | Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, 302 | const StaticTF transform) 303 | { 304 | //! Degrees to radians 305 | static constexpr double DEG_TO_RAD = (M_PI / 180.0); 306 | 307 | // Don't forget to convert from degrees to radians 308 | const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD); 309 | const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD); 310 | const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD); 311 | const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD); 312 | 313 | /** 314 | * @brief Compute transform from ECEF to ENU: 315 | * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates 316 | * ϕ = latitude 317 | * λ = longitude 318 | * The rotation is composed by a counter-clockwise rotation over the Z-axis 319 | * by an angle of 90 + λ followed by a counter-clockwise rotation over the east-axis by 320 | * an angle of 90 - ϕ. 321 | * R = [-sinλ cosλ 0.0 322 | * -cosλ*sinϕ -sinλ*sinϕ cosϕ 323 | * cosλ*cosϕ sinλ*cosϕ sinϕ ] 324 | * [East, North, Up] = R * [∂x, ∂y, ∂z] 325 | * where both [East, North, Up] and [∂x, ∂y, ∂z] are local coordinates relative to map origin. 326 | */ 327 | Eigen::Matrix3d R; 328 | R << -sin_lon, cos_lon, 0.0, 329 | -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat, 330 | cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; 331 | 332 | 333 | Eigen::Vector3d out; 334 | switch (transform) { 335 | case StaticTF::ECEF_TO_ENU: 336 | out = R * vec; 337 | break; 338 | 339 | case StaticTF::ENU_TO_ECEF: 340 | // ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose. 341 | R.transposeInPlace(); 342 | out = R * vec; 343 | break; 344 | 345 | default: 346 | break; 347 | } 348 | 349 | return out; 350 | } 351 | 352 | Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q) 353 | { 354 | Eigen::Affine3d transformation(q); 355 | return transformation * vec; 356 | } 357 | 358 | Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q) 359 | { 360 | Covariance3d cov_out_; 361 | EigenMapConstCovariance3d cov_in(cov.data()); 362 | EigenMapCovariance3d cov_out(cov_out_.data()); 363 | 364 | cov_out = cov_in * q; 365 | return cov_out_; 366 | } 367 | 368 | Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q) 369 | { 370 | Covariance6d cov_out_; 371 | Matrix6d R = Matrix6d::Zero(); 372 | 373 | EigenMapConstCovariance6d cov_in(cov.data()); 374 | EigenMapCovariance6d cov_out(cov_out_.data()); 375 | 376 | R.block<3, 3>(0, 0) = 377 | R.block<3, 3>(3, 3) = 378 | q.normalized().toRotationMatrix(); 379 | 380 | cov_out = R * cov_in * R.transpose(); 381 | return cov_out_; 382 | } 383 | 384 | Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q) 385 | { 386 | Covariance9d cov_out_; 387 | Matrix9d R = Matrix9d::Zero(); 388 | 389 | EigenMapConstCovariance9d cov_in(cov.data()); 390 | EigenMapCovariance9d cov_out(cov_out_.data()); 391 | 392 | R.block<3, 3>(0, 0) = 393 | R.block<3, 3>(3, 3) = 394 | R.block<3, 3>(6, 6) = 395 | q.normalized().toRotationMatrix(); 396 | 397 | cov_out = R * cov_in * R.transpose(); 398 | return cov_out_; 399 | } 400 | 401 | } // namespace frame_transforms 402 | } // namespace px4_ros_com 403 | -------------------------------------------------------------------------------- /test/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/px4_ros_com/86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af/test/__init__.py -------------------------------------------------------------------------------- /test/pipeline_io_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ################################################################################ 4 | # 5 | # Copyright 2019 PX4 Development Team. All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # 2. Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # 3. Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ################################################################################ 34 | 35 | # This script launches the microRTPS bridge pipeline to verify if there is 36 | # data being exchanged between the flight controller (in SITL) and companion 37 | # computer side, with its DDS participants 38 | 39 | import argparse 40 | import os 41 | from time import sleep 42 | from sys import exit 43 | from subprocess import call, CalledProcessError, check_output, DEVNULL, Popen, STDOUT 44 | 45 | if __name__ == "__main__": 46 | 47 | default_px4_ros_com_install_dir = os.path.expanduser("~") + "/PX4/px4_ros2_ws/install" 48 | default_px4_dir = os.path.expanduser("~") + "/PX4/Firmware" 49 | default_px4_target = "iris_rtps" 50 | default_test_dir = os.path.dirname(os.path.realpath(__file__)) 51 | default_test_type = "fcu_output" 52 | default_subscriber_topic = "sensor_combined" 53 | default_publisher_topic = "debug_vect" 54 | 55 | parser = argparse.ArgumentParser() 56 | parser.add_argument("-d", "--test-dir", dest='test_dir', type=str, 57 | help="Absolute path to test script", default=default_test_dir) 58 | parser.add_argument("-f", "--px4-firmware-dir", dest='px4_dir', type=str, 59 | help="Absolute path to PX4 Firmware dir, defaults to $HOME/PX4/Firmware", default=default_px4_dir) 60 | parser.add_argument("-b", "--px4-build-target", dest='px4_target', type=str, 61 | help="PX4 SITL target, defaults to iris_rtps", default=default_px4_target) 62 | parser.add_argument("-t", "--test-type", dest='test_type', type=str, 63 | help="Test type [fcu_input, fcu_output], defaults to 'fcu_output'", default=default_test_type) 64 | parser.add_argument("-s", "--subscriber", dest='subscriber_topic', type=str, 65 | help="ROS2 topic to test, defaults to 'sensor_combined'", default=default_subscriber_topic) 66 | parser.add_argument("-p", "--publisher", dest='publisher_topic', type=str, 67 | help="ROS2 publisher data, meaning the data to be received on the flight controller side, defaults to 'debug_vect'", default=default_publisher_topic) 68 | 69 | # Parse arguments 70 | args = parser.parse_args() 71 | if os.path.isabs(args.px4_dir): 72 | px4_dir = args.px4_dir 73 | else: 74 | raise Exception("Please provide PX4 Firmware absolute path") 75 | px4_target = args.px4_target 76 | test_dir = args.test_dir 77 | test_type = args.test_type 78 | listener = args.subscriber_topic 79 | advertiser = args.publisher_topic 80 | 81 | # get ROS distro 82 | ros_distro = check_output("rosversion -d", shell=True) 83 | 84 | # get PX4 Firmware tag 85 | px4_tag = check_output( 86 | "cd " + px4_dir + " && git describe --abbrev=0 && cd " + os.getcwd(), shell=True) 87 | 88 | print( 89 | "\n\033[34m-------------- PX4 XRCE-DDS COMMUNICATION TEST --------------\033[0m") 90 | print("\n-- Test configuration:\n") 91 | print(" > ROS 2 distro: \033[36m" + 92 | str(ros_distro.strip().decode("utf-8")).capitalize() + "\033[0m") 93 | print(" > PX4 Firmware: \033[36m" + px4_tag.decode() + "\033[0m") 94 | print("\033[5m-- Running " + ("Output test" if(test_type == 95 | "fcu_output") else "Input test") + "...\033[0m") 96 | 97 | # launch the micro-ros-agent 98 | print("\n\033[93m-- Starting micro-ros-agent..." + "\033[0m\n") 99 | call("micro-ros-agent udp4 --port 2019 &", shell=True, stderr=STDOUT) 100 | 101 | # waits for the agent to load 102 | sleep(3) 103 | 104 | # launch PX4 SITL daemon, in headless mode 105 | print( 106 | "\n\033[93m-- Starting the PX4 SITL daemon and Gazebo (without GUI)...\033[0m\n") 107 | call("cd " + px4_dir + " && (NO_PXH=1 HEADLESS=1 make px4_sitl_default gazebo_" + 108 | px4_target + " &) && cd " + os.getcwd(), shell=True, stderr=DEVNULL) 109 | 110 | # waits for PX4 daemon and Gazebo to load 111 | sleep(20) 112 | 113 | # setup the PX4 SITL bin dir 114 | px4_bin_dir = os.path.join(px4_dir, "build/px4_sitl_default/bin") 115 | 116 | # launch the specified test 117 | topic = "" 118 | test_format = list() 119 | test_result = -1 120 | 121 | if(test_type == "fcu_output"): 122 | # Flight controller output tests 123 | if (listener == "sensor_combined"): 124 | node = "sensor_combined_listener" 125 | topic = "sensor_combined" 126 | test_format = ["output", "from"] 127 | test_result = call( 128 | "python3 " + test_dir + "/test_output.py -t " + topic.replace("_", " ").title().replace(" ", ""), stderr=STDOUT, shell=True, 129 | universal_newlines=True) 130 | 131 | elif(test_type == "fcu_input"): 132 | # Flight controller input tests 133 | if (advertiser == "debug_vect"): 134 | package_name = "px4_ros_com" 135 | node = "debug_vect_advertiser" 136 | topic = "debug_vect" 137 | test_format = ["input", "on"] 138 | test_result = call( 139 | "python3 " + test_dir + "/test_input.py -b " + px4_bin_dir + " -p " + package_name + " -n " + node + " -t " + topic, stderr=STDOUT, shell=True, 140 | universal_newlines=True) 141 | elif (advertiser == "onboard_computer_status"): 142 | # specific test for https://github.com/Auterion/system_monitor_ros 143 | package_name = "system_monitor_ros" 144 | node = "system_monitor_node" 145 | topic = "onboard_computer_status" 146 | test_format = ["input", "on"] 147 | test_result = call( 148 | "python3 " + test_dir + "/test_input.py -b " + px4_bin_dir + " -p " + package_name + " -n " + node + " -t " + topic, stderr=STDOUT, shell=True, 149 | universal_newlines=True) 150 | 151 | call("killall gzserver micro-ros-agent px4 ros2", 152 | shell=True, stdout=DEVNULL, stderr=DEVNULL) 153 | 154 | print( 155 | "\033[34m------------------------ TEST RESULTS ------------------------\033[0m") 156 | if (test_result): 157 | print("\033[91m" + ("Output test" if(test_type == "fcu_output") else "Input test") + ": [FAILED]\tFlight controller " + test_format[0] + " test failed! Failed to get data " + test_format[1] + " the '" + 158 | topic + "' uORB topic\033[0m") 159 | print( 160 | "\033[34m" + "--------------------------------------------------------------" + "\033[0m\n") 161 | exit(1) 162 | else: 163 | print("\033[92m" + ("Output test" if(test_type == "fcu_output") else "Input test") + ": [SUCCESS]\tFlight controller " + test_format[0] + " test successfull! Successfully retrieved data " + test_format[1] + " the '" + 164 | topic + "' uORB topic\033[0m") 165 | print( 166 | "\033[34m--------------------------------------------------------------" + "\033[0m\n") 167 | exit(0) 168 | -------------------------------------------------------------------------------- /test/test_input.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ################################################################################ 4 | # 5 | # Copyright 2019 PX4 Development Team. All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # 2. Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # 3. Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ################################################################################ 34 | 35 | # This script tests the input data on a specific topic using the "listener" 36 | # app in PX4. If no output comes or there's a "never published" output 37 | # out of the app, the test fails. 38 | 39 | import argparse 40 | import os 41 | from sys import exit 42 | from subprocess import call, check_output, DEVNULL 43 | from time import sleep 44 | 45 | if __name__ == "__main__": 46 | parser = argparse.ArgumentParser() 47 | parser.add_argument("-b", "--px4-binary-dir", dest='px4_binary', type=str, 48 | help="Directory where the PX4 SITL daemon binaries are stored", required=True) 49 | parser.add_argument("-n", "--node-name", dest='node_name', type=str, 50 | help="Name of the node publishing the to the topic", required=True) 51 | parser.add_argument("-p", "--package-name", dest='package_name', type=str, 52 | help="ROS 2 package name of the node", required=True) 53 | parser.add_argument("-t", "--topic-name", dest='topic_name', type=str, 54 | help="Topic name to test the output to the autopilot", required=True) 55 | 56 | # Parse arguments 57 | args = parser.parse_args() 58 | 59 | test_cmd = "/bin/bash -c '" + \ 60 | os.path.join(args.px4_binary, 61 | "px4-listener") + " " + args.topic_name + "'" 62 | 63 | print("\n\033[93m-- " + args.topic_name + 64 | " uORB data test launched:\033[0m") 65 | # start the ROS 2 node 66 | call("ros2 run " + args.package_name + " " + args.node_name + " &", shell=True) 67 | 68 | sleep(3) 69 | 70 | # call the 'listener' command for the '' uORB topic 71 | output = check_output(test_cmd, shell=True, universal_newlines=True) 72 | 73 | call("killall " + args.node_name, 74 | shell=True, stdout=DEVNULL, stderr=DEVNULL) 75 | 76 | if output and "never published" not in output: 77 | print( 78 | "\n\033[42m-- Successfully obtained data on '" + args.topic_name + "' uORB topic. microRTPS bridge is up! Output:\033[0m") 79 | print("\033[97m" + output + "\033[0m") 80 | exit(0) 81 | else: 82 | print( 83 | "\n\033[41m-- Something went wrong. Please check if the microRTPS bridge is functioning correctly.\033[0m\n") 84 | exit(1) 85 | -------------------------------------------------------------------------------- /test/test_output.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ################################################################################ 4 | # 5 | # Copyright 2019 PX4 Development Team. All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # 1. Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # 2. Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # 3. Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | ################################################################################ 34 | 35 | # This script tests the output of 'ros2 topic echo /' 36 | # to evaluate if there's data output coming out of it. If the log output file 37 | # is empty, means that there's no output on the topic, so the test fails 38 | 39 | import argparse 40 | from os import remove 41 | from sys import exit 42 | from subprocess import call, TimeoutExpired 43 | 44 | if __name__ == "__main__": 45 | parser = argparse.ArgumentParser() 46 | parser.add_argument("-t", "--topic-name", dest='topic_name', type=str, 47 | help="Topic name to test the output to the autopilot", required=True) 48 | 49 | # Parse arguments 50 | args = parser.parse_args() 51 | 52 | timeout = 3 # seconds 53 | 54 | try: 55 | print( 56 | "\n\033[93m" + "-- " + args.topic_name + "_PubSubTopic output test launched:\033[0m") 57 | call("ros2 topic echo /" + args.topic_name + "_PubSubTopic", timeout=timeout, stdout=open( 58 | "ros2_topic_echo_out", "w"), shell=True) 59 | except TimeoutExpired as e: 60 | output = open("ros2_topic_echo_out", "r").read() 61 | if output: 62 | print( 63 | "\n\033[42m" + "-- Successfully obtained data on " + args.topic_name + "_PubSubTopic topic. microRTPS bridge is up! Output:\033[0m\n\n") 64 | print("\033[97m" + output + "\033[0m") 65 | remove("ros2_topic_echo_out") 66 | exit(0) 67 | else: 68 | print( 69 | "\n\033[41m" + "-- Something went wrong. Please check if the microRTPS bridge is functioning correctly.\033[0m\n") 70 | remove("ros2_topic_echo_out") 71 | exit(1) 72 | --------------------------------------------------------------------------------