From e6faac065646aa9a1c266215c5bd0c62edbf87dd Mon Sep 17 00:00:00 2001 From: Basti Date: Tue, 19 Oct 2021 16:23:47 +0200 Subject: [PATCH] Move examples to separate package inside repository. Signed-off-by: Basti --- README.md | 21 +---- examples/README.md | 32 ++++++++ examples/listeners/CMakeLists.txt | 25 ++++++ examples/listeners/README.md | 3 + examples/listeners/package.xml | 23 ++++++ .../src/01_sensor_combined_listener.cpp | 9 +-- .../src/02_vehicle_gps_position_listener.cpp | 9 +-- examples/offboard/CMakeLists.txt | 21 +++++ examples/offboard/README.md | 3 + examples/offboard/package.xml | 23 ++++++ .../offboard/src}/offboard_control.cpp | 34 +++++---- examples/publishers/CMakeLists.txt | 37 +++++++++ examples/publishers/README.md | 3 + examples/publishers/package.xml | 23 ++++++ .../src/01_debug_vect_publisher.cpp | 13 ++-- examples/publishers/src/02_arm_publisher.cpp | 68 +++++++++++++++++ .../publishers/src/03_disarm_publisher.cpp | 68 +++++++++++++++++ .../publishers/src/04_takeoff_publisher.cpp | 67 ++++++++++++++++ examples/publishers/src/05_land_publisher.cpp | 72 ++++++++++++++++++ examples/publishers/src/common.h | 65 ++++++++++++++++ CMakeLists.txt => px4_ros_com/CMakeLists.txt | 16 ---- LICENSE => px4_ros_com/LICENSE | 0 px4_ros_com/README.md | 20 +++++ .../cmake}/EnableC++XX.cmake | 0 .../cmake}/EnableSanitizers.cmake | 0 .../cmake}/GenerateMicroRTPSAgent.cmake | 0 .../include}/px4_ros_com/frame_transforms.h | 0 .../sensor_combined_listener.launch.py | 0 package.xml => px4_ros_com/package.xml | 0 {scripts => px4_ros_com/scripts}/__init__.py | 0 ...x_generate_uorb_topic_files.cpython-38.pyc | Bin 0 -> 13570 bytes .../uorb_rtps_classifier.cpython-38.pyc | Bin 0 -> 6323 bytes .../scripts}/build_all.bash | 0 .../scripts}/build_ros1_bridge.bash | 0 .../scripts}/build_ros2_workspace.bash | 0 .../scripts}/clean_all.bash | 0 .../scripts}/generate_microRTPS_bridge.py | 0 .../scripts}/px_generate_uorb_topic_files.py | 0 .../scripts}/px_generate_uorb_topic_helper.py | 0 .../scripts}/setup_system.bash | 0 .../scripts}/uorb_rtps_classifier.py | 0 .../src}/lib/frame_transforms.cpp | 0 .../templates}/Publisher.cpp.em | 0 .../templates}/Publisher.h.em | 0 .../templates}/RtpsTopics.cpp.em | 0 .../templates}/RtpsTopics.h.em | 0 .../templates}/Subscriber.cpp.em | 0 .../templates}/Subscriber.h.em | 0 .../templates}/microRTPS_agent.cpp.em | 0 .../templates}/microRTPS_timesync.cpp.em | 0 .../templates}/microRTPS_timesync.h.em | 0 .../templates}/microRTPS_transport.cpp | 0 .../templates}/microRTPS_transport.h | 0 .../templates}/urtps_bridge_topics.yaml | 0 {test => px4_ros_com/test}/__init__.py | 0 .../test}/pipeline_io_test.py | 0 {test => px4_ros_com/test}/test_input.py | 0 {test => px4_ros_com/test}/test_output.py | 0 58 files changed, 586 insertions(+), 69 deletions(-) create mode 100644 examples/README.md create mode 100644 examples/listeners/CMakeLists.txt create mode 100644 examples/listeners/README.md create mode 100644 examples/listeners/package.xml rename src/examples/listeners/sensor_combined_listener.cpp => examples/listeners/src/01_sensor_combined_listener.cpp (95%) rename src/examples/listeners/vehicle_gps_position_listener.cpp => examples/listeners/src/02_vehicle_gps_position_listener.cpp (95%) create mode 100644 examples/offboard/CMakeLists.txt create mode 100644 examples/offboard/README.md create mode 100644 examples/offboard/package.xml rename {src/examples/offboard => examples/offboard/src}/offboard_control.cpp (92%) create mode 100644 examples/publishers/CMakeLists.txt create mode 100644 examples/publishers/README.md create mode 100644 examples/publishers/package.xml rename src/examples/advertisers/debug_vect_advertiser.cpp => examples/publishers/src/01_debug_vect_publisher.cpp (92%) create mode 100644 examples/publishers/src/02_arm_publisher.cpp create mode 100644 examples/publishers/src/03_disarm_publisher.cpp create mode 100644 examples/publishers/src/04_takeoff_publisher.cpp create mode 100644 examples/publishers/src/05_land_publisher.cpp create mode 100644 examples/publishers/src/common.h rename CMakeLists.txt => px4_ros_com/CMakeLists.txt (89%) rename LICENSE => px4_ros_com/LICENSE (100%) create mode 100644 px4_ros_com/README.md rename {cmake => px4_ros_com/cmake}/EnableC++XX.cmake (100%) rename {cmake => px4_ros_com/cmake}/EnableSanitizers.cmake (100%) rename {cmake => px4_ros_com/cmake}/GenerateMicroRTPSAgent.cmake (100%) rename {include => px4_ros_com/include}/px4_ros_com/frame_transforms.h (100%) rename {launch => px4_ros_com/launch}/sensor_combined_listener.launch.py (100%) rename package.xml => px4_ros_com/package.xml (100%) rename {scripts => px4_ros_com/scripts}/__init__.py (100%) create mode 100644 px4_ros_com/scripts/__pycache__/px_generate_uorb_topic_files.cpython-38.pyc create mode 100644 px4_ros_com/scripts/__pycache__/uorb_rtps_classifier.cpython-38.pyc rename {scripts => px4_ros_com/scripts}/build_all.bash (100%) rename {scripts => px4_ros_com/scripts}/build_ros1_bridge.bash (100%) rename {scripts => px4_ros_com/scripts}/build_ros2_workspace.bash (100%) rename {scripts => px4_ros_com/scripts}/clean_all.bash (100%) rename {scripts => px4_ros_com/scripts}/generate_microRTPS_bridge.py (100%) rename {scripts => px4_ros_com/scripts}/px_generate_uorb_topic_files.py (100%) rename {scripts => px4_ros_com/scripts}/px_generate_uorb_topic_helper.py (100%) rename {scripts => px4_ros_com/scripts}/setup_system.bash (100%) rename {scripts => px4_ros_com/scripts}/uorb_rtps_classifier.py (100%) rename {src => px4_ros_com/src}/lib/frame_transforms.cpp (100%) rename {templates => px4_ros_com/templates}/Publisher.cpp.em (100%) rename {templates => px4_ros_com/templates}/Publisher.h.em (100%) rename {templates => px4_ros_com/templates}/RtpsTopics.cpp.em (100%) rename {templates => px4_ros_com/templates}/RtpsTopics.h.em (100%) rename {templates => px4_ros_com/templates}/Subscriber.cpp.em (100%) rename {templates => px4_ros_com/templates}/Subscriber.h.em (100%) rename {templates => px4_ros_com/templates}/microRTPS_agent.cpp.em (100%) rename {templates => px4_ros_com/templates}/microRTPS_timesync.cpp.em (100%) rename {templates => px4_ros_com/templates}/microRTPS_timesync.h.em (100%) rename {templates => px4_ros_com/templates}/microRTPS_transport.cpp (100%) rename {templates => px4_ros_com/templates}/microRTPS_transport.h (100%) rename {templates => px4_ros_com/templates}/urtps_bridge_topics.yaml (100%) rename {test => px4_ros_com/test}/__init__.py (100%) rename {test => px4_ros_com/test}/pipeline_io_test.py (100%) rename {test => px4_ros_com/test}/test_input.py (100%) rename {test => px4_ros_com/test}/test_output.py (100%) diff --git a/README.md b/README.md index 6847c10e..89524f7f 100644 --- a/README.md +++ b/README.md @@ -1,20 +1,3 @@ -# PX4-ROS2 bridge +# tbd -[![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) - -This package materializes the ROS2 side of PX4-FastRTPS/DDS bridge, establishing a bridge between the PX4 autopilot stack through a micro-RTPS bridge, Fast-RTPS(DDS) and ROS2. It has a straight dependency on the [`px4_msgs`](https://github.com/PX4/px4_msgs) package, as it depends on the IDL files, to generate the micro-RTPS bridge agent, and on the ROS interfaces and typesupport, to allow building and running the example nodes. - -The [`master`](https://github.com/PX4/px4_ros_com/tree/master) branch of this package composes the ROS2 package and the ROS2 side (agent) of the bridge. The [`ros1`](https://github.com/PX4/px4_ros_com/tree/ros1) branch is a product of the `master` and represents the ROS(1) package and the ROS(1) side of the bridge, for wich it is required using the [`ros1_bridge`](https://github.com/ros2/ros1_bridge). - -## Install, build and usage - -Check the [RTPS/ROS2 Interface](https://dev.px4.io/en/middleware/micrortps.html) section on the PX4 Devguide for details on how to install the required dependencies, build the package (composed by the two branches) and use it. - -## Bug tracking and feature requests - -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). - -## Questions and troubleshooting - -Reach the PX4 development team on the `#messaging` or `#ros` PX4 Slack channels: -[![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io) +main repo readme \ No newline at end of file diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 00000000..7ed25310 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,32 @@ +# Examples + +This directory contains a set of examples application for the PX4-ROS2 bridge. All examples are intended to demonstrate basic interactions between PX4 and ROS2. Feel free to play with the code, extend it and copy it into your own project. + +It is assumed that a reads has a basic understanding of running ROS2 applications as well as PX4. + +# Layout + +There are several separate examples. Each example assembles its own executable and can be used standalone to interact with a PX4 vehicle from your ROS2 environment. + +The examples are structured: + +* `listeners` + + ... + +* `publisher` ... + +* `offboard` ... + + +# Using Examples + +... + +# Copying Examples + +... + +# Contributing Examples + +... diff --git a/examples/listeners/CMakeLists.txt b/examples/listeners/CMakeLists.txt new file mode 100644 index 00000000..f1b04551 --- /dev/null +++ b/examples/listeners/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_ros_com_listener_examples) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) + +add_executable(sensor_combined_listener src/01_sensor_combined_listener.cpp) +ament_target_dependencies(sensor_combined_listener rclcpp px4_msgs) + +add_executable(vehicle_gps_position_listener src/02_vehicle_gps_position_listener.cpp) +ament_target_dependencies(vehicle_gps_position_listener rclcpp px4_msgs) + +ament_package() + +install( + TARGETS + sensor_combined_listener + vehicle_gps_position_listener + DESTINATION + lib/${PROJECT_NAME}) diff --git a/examples/listeners/README.md b/examples/listeners/README.md new file mode 100644 index 00000000..23bebf33 --- /dev/null +++ b/examples/listeners/README.md @@ -0,0 +1,3 @@ +# tbd + +listeners readme \ No newline at end of file diff --git a/examples/listeners/package.xml b/examples/listeners/package.xml new file mode 100644 index 00000000..7f886a7a --- /dev/null +++ b/examples/listeners/package.xml @@ -0,0 +1,23 @@ + + + + px4_ros_com_listener_examples + 1.0.0 + Example of ROS2 and Px4 interaction - listeners + BSD 3-Clause + Nuno Marques + Nuno Marques + Bastian Jäger + + ament_cmake + + rclcpp + px4_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/examples/listeners/sensor_combined_listener.cpp b/examples/listeners/src/01_sensor_combined_listener.cpp similarity index 95% rename from src/examples/listeners/sensor_combined_listener.cpp rename to examples/listeners/src/01_sensor_combined_listener.cpp index d42e8607..104afb84 100644 --- a/src/examples/listeners/sensor_combined_listener.cpp +++ b/examples/listeners/src/01_sensor_combined_listener.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * 2018 PX4 Pro Development Team. All rights reserved. + * 2021 PX4 Pro Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: @@ -33,7 +33,7 @@ /** * @brief Sensor Combined uORB topic listener example - * @file sensor_combined_listener.cpp + * @file 01_sensor_combined_listener.cpp * @addtogroup examples * @author Nuno Marques * @author Vicente Monge @@ -50,10 +50,7 @@ class SensorCombinedListener : public rclcpp::Node public: explicit SensorCombinedListener() : Node("sensor_combined_listener") { subscription_ = this->create_subscription( - "fmu/sensor_combined/out", -#ifdef ROS_DEFAULT_API - 10, -#endif + "fmu/sensor_combined/out", 10, [this](const px4_msgs::msg::SensorCombined::UniquePtr msg) { 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"; std::cout << "RECEIVED SENSOR COMBINED DATA" << std::endl; diff --git a/src/examples/listeners/vehicle_gps_position_listener.cpp b/examples/listeners/src/02_vehicle_gps_position_listener.cpp similarity index 95% rename from src/examples/listeners/vehicle_gps_position_listener.cpp rename to examples/listeners/src/02_vehicle_gps_position_listener.cpp index 643c16c1..3db99c10 100644 --- a/src/examples/listeners/vehicle_gps_position_listener.cpp +++ b/examples/listeners/src/02_vehicle_gps_position_listener.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright 2019 PX4 Development Team. All rights reserved. + * Copyright 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: @@ -32,7 +32,7 @@ /** * @brief Vehicle GPS position uORB topic listener example - * @file vehicle_global_position_listener.cpp + * @file 02_vehicle_global_position_listener.cpp * @addtogroup examples * @author Nuno Marques */ @@ -48,10 +48,7 @@ class VehicleGpsPositionListener : public rclcpp::Node public: explicit VehicleGpsPositionListener() : Node("vehicle_global_position_listener") { subscription_ = this->create_subscription( - "fmu/vehicle_gps_position/out", -#ifdef ROS_DEFAULT_API - 10, -#endif + "fmu/vehicle_gps_position/out", 10, [this](const px4_msgs::msg::VehicleGpsPosition::UniquePtr msg) { std::cout << "\n\n\n\n\n\n\n\n\n\n"; std::cout << "RECEIVED VEHICLE GPS POSITION DATA" << std::endl; diff --git a/examples/offboard/CMakeLists.txt b/examples/offboard/CMakeLists.txt new file mode 100644 index 00000000..9bb90b0f --- /dev/null +++ b/examples/offboard/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_ros_com_offboard_example) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) + +add_executable(offboard_control src/offboard_control.cpp) +ament_target_dependencies(offboard_control rclcpp px4_msgs) + +ament_package() + +install( + TARGETS + offboard_control + DESTINATION + lib/${PROJECT_NAME}) diff --git a/examples/offboard/README.md b/examples/offboard/README.md new file mode 100644 index 00000000..6f2d7fba --- /dev/null +++ b/examples/offboard/README.md @@ -0,0 +1,3 @@ +# tbd + +offbaord readme \ No newline at end of file diff --git a/examples/offboard/package.xml b/examples/offboard/package.xml new file mode 100644 index 00000000..e8c76c48 --- /dev/null +++ b/examples/offboard/package.xml @@ -0,0 +1,23 @@ + + + + px4_ros_com_offboard_example + 1.0.0 + Example of ROS2 and Px4 interaction - offboard + BSD 3-Clause + Nuno Marques + Nuno Marques + Bastian Jäger + + ament_cmake + + rclcpp + px4_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/examples/offboard/offboard_control.cpp b/examples/offboard/src/offboard_control.cpp similarity index 92% rename from src/examples/offboard/offboard_control.cpp rename to examples/offboard/src/offboard_control.cpp index bedcf098..69c72572 100644 --- a/src/examples/offboard/offboard_control.cpp +++ b/examples/offboard/src/offboard_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright 2020 PX4 Development Team. All rights reserved. + * Copyright 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: @@ -61,21 +61,12 @@ using namespace px4_msgs::msg; class OffboardControl : public rclcpp::Node { public: OffboardControl() : Node("offboard_control") { -#ifdef ROS_DEFAULT_API offboard_control_mode_publisher_ = this->create_publisher("fmu/offboard_control_mode/in", 10); trajectory_setpoint_publisher_ = this->create_publisher("fmu/trajectory_setpoint/in", 10); vehicle_command_publisher_ = this->create_publisher("fmu/vehicle_command/in", 10); -#else - offboard_control_mode_publisher_ = - this->create_publisher("fmu/offboard_control_mode/in"); - trajectory_setpoint_publisher_ = - this->create_publisher("fmu/trajectory_setpoint/in"); - vehicle_command_publisher_ = - this->create_publisher("fmu/vehicle_command/in"); -#endif // get common timestamp timesync_sub_ = @@ -84,6 +75,15 @@ class OffboardControl : public rclcpp::Node { timestamp_.store(msg->timestamp); }); + + this->declare_parameter("pos_x_m"); + this->declare_parameter("pos_y_m"); + this->declare_parameter("pos_z_m"); + + this->get_parameter_or("pos_x_m", pos_x_m_, 0.f); + this->get_parameter_or("pos_y_m", pos_y_m_, 0.f); + this->get_parameter_or("pos_z_m", pos_z_m_, -5.f); + offboard_setpoint_counter_ = 0; auto timer_callback = [this]() -> void { @@ -93,7 +93,7 @@ class OffboardControl : public rclcpp::Node { this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); // Arm the vehicle - this->arm(); + //this->arm(); } // offboard_control_mode needs to be paired with trajectory_setpoint @@ -127,6 +127,10 @@ class OffboardControl : public rclcpp::Node { void publish_trajectory_setpoint() const; void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0) const; + + float pos_x_m_{0.f}; + float pos_y_m_{0.f}; + float pos_z_m_{0.f}; }; /** @@ -172,10 +176,10 @@ void OffboardControl::publish_offboard_control_mode() const { void OffboardControl::publish_trajectory_setpoint() const { TrajectorySetpoint msg{}; msg.timestamp = timestamp_.load(); - msg.x = 0.0; - msg.y = 0.0; - msg.z = -5.0; - msg.yaw = -3.14; // [-PI:PI] + msg.x = pos_x_m_; + msg.y = pos_y_m_; + msg.z = pos_z_m_; + msg.yaw = NAN; // [-PI:PI] trajectory_setpoint_publisher_->publish(msg); } diff --git a/examples/publishers/CMakeLists.txt b/examples/publishers/CMakeLists.txt new file mode 100644 index 00000000..94b4d4f7 --- /dev/null +++ b/examples/publishers/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_ros_com_publisher_examples) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) + +add_executable(debug_vect_publisher src/01_debug_vect_publisher.cpp) +ament_target_dependencies(debug_vect_publisher rclcpp px4_msgs) + +add_executable(arm_publisher src/02_arm_publisher.cpp) +ament_target_dependencies(arm_publisher rclcpp px4_msgs) + +add_executable(disarm_publisher src/03_disarm_publisher.cpp) +ament_target_dependencies(disarm_publisher rclcpp px4_msgs) + +add_executable(takeoff_publisher src/04_takeoff_publisher.cpp) +ament_target_dependencies(takeoff_publisher rclcpp px4_msgs) + +add_executable(land_publisher src/05_land_publisher.cpp) +ament_target_dependencies(land_publisher rclcpp px4_msgs) + +ament_package() + +install( + TARGETS + debug_vect_publisher + arm_publisher + disarm_publisher + takeoff_publisher + land_publisher + DESTINATION + lib/${PROJECT_NAME}) diff --git a/examples/publishers/README.md b/examples/publishers/README.md new file mode 100644 index 00000000..083b665d --- /dev/null +++ b/examples/publishers/README.md @@ -0,0 +1,3 @@ +# tbd + +publishers readme \ No newline at end of file diff --git a/examples/publishers/package.xml b/examples/publishers/package.xml new file mode 100644 index 00000000..8b71b243 --- /dev/null +++ b/examples/publishers/package.xml @@ -0,0 +1,23 @@ + + + + px4_ros_com_publisher_examples + 0.1.0 + Example of ROS2 and Px4 interaction - advertisers + BSD 3-Clause + Nuno Marques + Nuno Marques + Bastian Jäger + + ament_cmake + + rclcpp + px4_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/examples/advertisers/debug_vect_advertiser.cpp b/examples/publishers/src/01_debug_vect_publisher.cpp similarity index 92% rename from src/examples/advertisers/debug_vect_advertiser.cpp rename to examples/publishers/src/01_debug_vect_publisher.cpp index 2fbcef72..60a80739 100644 --- a/src/examples/advertisers/debug_vect_advertiser.cpp +++ b/examples/publishers/src/01_debug_vect_publisher.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright 2018 PX4 Development Team. All rights reserved. + * Copyright 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: @@ -31,27 +31,26 @@ ****************************************************************************/ /** - * @brief Debug Vect uORB topic adverstiser example - * @file debug_vect_advertiser.cpp + * @brief Debug Vect uORB topic publisher example + * @file 01_debug_vect_advertiser.cpp * @addtogroup examples * @author Nuno Marques + * @author Bastian Jäger */ #include #include #include +#include "common.h" + using namespace std::chrono_literals; class DebugVectAdvertiser : public rclcpp::Node { public: DebugVectAdvertiser() : Node("debug_vect_advertiser") { -#ifdef ROS_DEFAULT_API publisher_ = this->create_publisher("fmu/debug_vect/in", 10); -#else - publisher_ = this->create_publisher("fmu/debug_vect/in"); -#endif auto timer_callback = [this]()->void { auto debug_vect = px4_msgs::msg::DebugVect(); diff --git a/examples/publishers/src/02_arm_publisher.cpp b/examples/publishers/src/02_arm_publisher.cpp new file mode 100644 index 00000000..4c766c36 --- /dev/null +++ b/examples/publishers/src/02_arm_publisher.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @brief Publishing set mode message for `arm` towards PX4 + * @file 02_arm_publisher.cpp + * @addtogroup examples + * @author Nuno Marques + * @author Bastian Jäger + */ + +#include +#include +#include + +#include "common.h" + +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("arm_publisher"); + + rclcpp::Publisher::SharedPtr vehicle_command_publisher + = node->create_publisher("fmu/vehicle_command/in", 10); + + rclcpp::WallRate loop_rate(50ms); + + for (int i = 0; i < 10; ++i) { + publish_vehicle_command(vehicle_command_publisher, VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1); + loop_rate.sleep(); + } + + + rclcpp::shutdown(); + return 0; +} + diff --git a/examples/publishers/src/03_disarm_publisher.cpp b/examples/publishers/src/03_disarm_publisher.cpp new file mode 100644 index 00000000..86990925 --- /dev/null +++ b/examples/publishers/src/03_disarm_publisher.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @brief Publishing set mode message for `disarm` towards PX4 + * @file 03_disarm_publisher.cpp + * @addtogroup examples + * @author Nuno Marques + * @author Bastian Jäger + */ + +#include +#include +#include + +#include "common.h" + +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("arm_publisher"); + + rclcpp::Publisher::SharedPtr vehicle_command_publisher + = node->create_publisher("fmu/vehicle_command/in", 10); + + rclcpp::WallRate loop_rate(50ms); + + for (int i = 0; i < 10; ++i) { + publish_vehicle_command(vehicle_command_publisher, VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0); + loop_rate.sleep(); + } + + + rclcpp::shutdown(); + return 0; +} + diff --git a/examples/publishers/src/04_takeoff_publisher.cpp b/examples/publishers/src/04_takeoff_publisher.cpp new file mode 100644 index 00000000..f6ffc825 --- /dev/null +++ b/examples/publishers/src/04_takeoff_publisher.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @brief Publishing set mode message for `takeoff` towards PX4 + * @file 04_takeoff_publisher.cpp + * @addtogroup examples + * @author Mickey Cowden + * @author Nuno Marques + * @author Bastian Jäger + */ + +#include +#include +#include + +#include "common.h" + +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("takeoff_publisher"); + + rclcpp::Publisher::SharedPtr vehicle_command_publisher + = node->create_publisher("fmu/vehicle_command/in", 10); + rclcpp::WallRate loop_rate(50ms); + + for (int i = 0; i < 10; ++i) { + publish_vehicle_command(vehicle_command_publisher, VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 4, 2); + loop_rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} + diff --git a/examples/publishers/src/05_land_publisher.cpp b/examples/publishers/src/05_land_publisher.cpp new file mode 100644 index 00000000..4ab1c28e --- /dev/null +++ b/examples/publishers/src/05_land_publisher.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @brief Publishing set mode message for `land` towards PX4 + * @file 05_land_publisher.cpp + * @addtogroup examples + * @author Mickey Cowden + * @author Nuno Marques + * @author Bastian Jäger + + * The TrajectorySetpoint message and the OFFBOARD mode in general are under an ongoing update. + * Please refer to PR: https://github.com/PX4/PX4-Autopilot/pull/16739 for more info. + * As per PR: https://github.com/PX4/PX4-Autopilot/pull/17094, the format + * of the TrajectorySetpoint message shall change. + */ + +#include +#include +#include + +#include "common.h" + +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("land_publisher"); + + rclcpp::Publisher::SharedPtr vehicle_command_publisher + = node->create_publisher("fmu/vehicle_command/in", 10); + rclcpp::WallRate loop_rate(50ms); + + for (int i = 0; i < 10; ++i) { + publish_vehicle_command(vehicle_command_publisher, VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 4, 6); + loop_rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} + diff --git a/examples/publishers/src/common.h b/examples/publishers/src/common.h new file mode 100644 index 00000000..96687138 --- /dev/null +++ b/examples/publishers/src/common.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @brief Common functions header for px4_ros_com_publisher_examples + * @file common.h + * @addtogroup examples + * @author Bastian Jäger + */ + +#include +#include + +using namespace px4_msgs::msg; + +/** + * @brief Publish vehicle commands + * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) + * @param param1 Command parameter 1 + * @param param2 Command parameter 2 + */ +void publish_vehicle_command(const rclcpp::Publisher::SharedPtr& vehicle_command_publisher, + uint16_t command, float param1 , float param2 = 0.f, float param3 = 0.f) { + VehicleCommand msg { }; + msg.param1 = param1; + msg.param2 = param2; + msg.param3 = param3; + msg.command = command; + msg.target_system = 1; + msg.target_component = 1; + msg.source_system = 1; + msg.source_component = 1; + msg.from_external = true; + + vehicle_command_publisher->publish(msg); +} diff --git a/CMakeLists.txt b/px4_ros_com/CMakeLists.txt similarity index 89% rename from CMakeLists.txt rename to px4_ros_com/CMakeLists.txt index 39016f38..315fd8ee 100644 --- a/CMakeLists.txt +++ b/px4_ros_com/CMakeLists.txt @@ -67,16 +67,6 @@ include(GenerateMicroRTPSAgent) include_directories(include) -function(custom_executable subfolder target) - add_executable(${target} src/${subfolder}/${target}.cpp) - ament_target_dependencies(${target} - rclcpp - px4_msgs - ) - install(TARGETS ${target} - DESTINATION lib/${PROJECT_NAME}) -endfunction() - # Add frame_transforms lib add_library(frame_transforms SHARED src/lib/frame_transforms.cpp) ament_target_dependencies(frame_transforms Eigen3 geometry_msgs sensor_msgs) @@ -92,12 +82,6 @@ add_executable(micrortps_agent ${MICRORTPS_AGENT_FILES}) ament_target_dependencies(micrortps_agent fastcdr fastrtps rclcpp) target_link_libraries(micrortps_agent fastcdr fastrtps ${rclcpp_LIBRARIES}) -# Add examples -custom_executable(examples/listeners sensor_combined_listener) -custom_executable(examples/listeners vehicle_gps_position_listener) -custom_executable(examples/advertisers debug_vect_advertiser) -custom_executable(examples/offboard offboard_control) - ############ # Install ## ############ diff --git a/LICENSE b/px4_ros_com/LICENSE similarity index 100% rename from LICENSE rename to px4_ros_com/LICENSE diff --git a/px4_ros_com/README.md b/px4_ros_com/README.md new file mode 100644 index 00000000..6847c10e --- /dev/null +++ b/px4_ros_com/README.md @@ -0,0 +1,20 @@ +# PX4-ROS2 bridge + +[![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) + +This package materializes the ROS2 side of PX4-FastRTPS/DDS bridge, establishing a bridge between the PX4 autopilot stack through a micro-RTPS bridge, Fast-RTPS(DDS) and ROS2. It has a straight dependency on the [`px4_msgs`](https://github.com/PX4/px4_msgs) package, as it depends on the IDL files, to generate the micro-RTPS bridge agent, and on the ROS interfaces and typesupport, to allow building and running the example nodes. + +The [`master`](https://github.com/PX4/px4_ros_com/tree/master) branch of this package composes the ROS2 package and the ROS2 side (agent) of the bridge. The [`ros1`](https://github.com/PX4/px4_ros_com/tree/ros1) branch is a product of the `master` and represents the ROS(1) package and the ROS(1) side of the bridge, for wich it is required using the [`ros1_bridge`](https://github.com/ros2/ros1_bridge). + +## Install, build and usage + +Check the [RTPS/ROS2 Interface](https://dev.px4.io/en/middleware/micrortps.html) section on the PX4 Devguide for details on how to install the required dependencies, build the package (composed by the two branches) and use it. + +## Bug tracking and feature requests + +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). + +## Questions and troubleshooting + +Reach the PX4 development team on the `#messaging` or `#ros` PX4 Slack channels: +[![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io) diff --git a/cmake/EnableC++XX.cmake b/px4_ros_com/cmake/EnableC++XX.cmake similarity index 100% rename from cmake/EnableC++XX.cmake rename to px4_ros_com/cmake/EnableC++XX.cmake diff --git a/cmake/EnableSanitizers.cmake b/px4_ros_com/cmake/EnableSanitizers.cmake similarity index 100% rename from cmake/EnableSanitizers.cmake rename to px4_ros_com/cmake/EnableSanitizers.cmake diff --git a/cmake/GenerateMicroRTPSAgent.cmake b/px4_ros_com/cmake/GenerateMicroRTPSAgent.cmake similarity index 100% rename from cmake/GenerateMicroRTPSAgent.cmake rename to px4_ros_com/cmake/GenerateMicroRTPSAgent.cmake diff --git a/include/px4_ros_com/frame_transforms.h b/px4_ros_com/include/px4_ros_com/frame_transforms.h similarity index 100% rename from include/px4_ros_com/frame_transforms.h rename to px4_ros_com/include/px4_ros_com/frame_transforms.h diff --git a/launch/sensor_combined_listener.launch.py b/px4_ros_com/launch/sensor_combined_listener.launch.py similarity index 100% rename from launch/sensor_combined_listener.launch.py rename to px4_ros_com/launch/sensor_combined_listener.launch.py diff --git a/package.xml b/px4_ros_com/package.xml similarity index 100% rename from package.xml rename to px4_ros_com/package.xml diff --git a/scripts/__init__.py b/px4_ros_com/scripts/__init__.py similarity index 100% rename from scripts/__init__.py rename to px4_ros_com/scripts/__init__.py diff --git a/px4_ros_com/scripts/__pycache__/px_generate_uorb_topic_files.cpython-38.pyc b/px4_ros_com/scripts/__pycache__/px_generate_uorb_topic_files.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..746aadfa8a4e63951052b4335e13a5458574051b GIT binary patch literal 13570 zcmb_iTW}m#TJGD-^jtK$*p@BFN!rfE9w(A)CvhCd>$PLcjyJaKwdGtom-cv0OB!iz zI^APiv#o_%g|(q75H3q`2{3}NtSNY4*gec*vd2vh-G6jT8Zo2q@_fd_C=Yrg;Q zo{J&Dm{V6GYv%%imeD$ zXcg5~`K{R+zja&Zw_zLnPS}Z7j$=tX`G!(^$57nVVn+U&s@SHgw2r%Jp}QH^TvfL# ztE#FXZN%?H{7$aRPqeclwT7B+C@&~Nv(2~7H zJq_WTGL?LF&>p0b)ZUA}>qAx6`ufX$PJg*C+MgJ(heX~U7K8SP7_zsBVKFkN*;~aH zu@&!aVw)JndsJ+nQ|;|yhrL6L*<)g-y;D45KXO(PyTqgKE8@{r&E6#*6OX^I*pCWJ z?8f^su}3_C_v7M8@f6;ccv|emd$-sp3V81k`^7VOKOvqK<9I(QCd4G(Pl*HKAl^@l zMR7|SQr;y630JK z?Fn~cVN!fqoDe6)D<9~P0ZN||r`M&Y#2Il`yxJ{4C%z(H6X(T+4~)3Qw74iPiLVMx zT>c;t=Uovq;%Z;3*Toy|4)LaVtJ}&JZ;N;OYP>7n6HZ_HnkWiGT<>d95<)zJv0WE# zx1U)t$9eOey!L@L#XjgBTsZWhE>F16P|V$q@VQm`!h;LX+m9f80i`oSwGY=GL+VAO zu8R~>Ls$o8mX54g;P=6WBSKqDqsHH(=8@CNyN}r~xsSVt-ItI)isv~zFT2}UldAHe zzVyDkpX%W`=5BKjk5Qj5uM)5JB1ZL(;rSAtc78CK}6`vvHwkl_pD#hBfaNg)1k$ zdQ+BMi>q0)b!kysi`#nEtgKh9iK;he?Yn&OisdcW{Nk-brs{fLaV~C@`IPk2bZgt0 zBDxZ2YL%;vy7Vo#dejP)*6z3JO{-d5wrXx%T=V>5rDBzRtLc?%b4OddEc_bf#&cFV zE*l?jdaeYC#`32s=(ol;k;m6!WJj&mlMid%SeA8fJgQev!_*B|dgXepwQCctMzOS5 zoTIT=t=$hL*QwOncg2-+?y~iYTPb?;&ssC{^=i?x&Ni=CT=^`ri@x<*)2+?5#!uBT zit^mNZ|yr(unrwK_}n;x!`7uYpSMoCH{43SQFUv+HRBem6Sn@!mD8aXLR{9BR=HjJur>c6A~Y{}b5~0BhAXLs)De8DA($>CL(_3;#j5K#Va9Q)bU>hjd7sdKMS-Bo0sh*5Cwok=juoxEQ3{Bp52Dcl>ArFx}QuQ@k8r#?Gd zE|rUwNl%t0@p%s9!2v5Ly^<_9d=Gh_xoBh(T{w6=ikPZ_Kj)R`YdSwFqvsSJ?-~MM z31F93wG}l`+v=LKqRlBQdZ4v6p{=RV8HZMkz-a5fv8GB3u>|TQfFbz^v3lLqZRP39nHp`(XwPQ~KV`vOgc@BErth#n+~$QNaht!jyCym14;alkP3w zt)XG5Qukb%Owu4Uh%iy7XlVGT5+sdaLTMp?-E`2@031crLCpyGypKs~tARr7Usb_E zy>*1ya-p~gzE#SeZ`EflEQnYYmTQ(j?^<)^8*a@KW$Bjux?JWa>v|)8Ch%OrMv_cSPpm9dj=^}s3H@*V>TDGP@6aqYO}SCY{(p=K|xp>$CrtA zBxRbCfktHxVF`nwoJLX?HHM#~pgJNzO(YgI?~h|D$6_i!BpOU58B$b0cc6zy0yJ6NUW(Vxtf7*%JFxO;ycYCw+3Lr+}KWOaACmT5oA|Ykp|>aTq3m;$Zv!TkB2RkXU%dRTMTRFr3h|nW@kd=A&7|YF>(rZmqv<;YB^ZnxO;6XYyToh{eOKFsxQwiG@TDgWLCtmQH)iCS1MG5WKK)W!D zbqEtwW9?{o4Yw2~BeWF`BOCg#2qh_;Swa?t-C;65e9fW}B6YLqp|}$-P*^p}~VOVKyVBJwl`j7W5{{9%vM$ zM?;|r^1@`ZA<$E3u$DK7V3Sf*f{j7ArCcjjn!=^wK;1i{T=K&l4+fc}CBtmzJHXCRhmi@B#Re6H!>2AzUzxdl z^4#>)Y3I!OlUH616J-xtE;Or9>xg)vu~5fU49rYjxOD#H%#?HH-1(_+@Z!~(OIK$& z;Y__b6K?H_>-A;F2kNZgOAEtc28g&?^qsP}C3g|u(vdh*AE^j5x?#4{fO4~aA0#KK zNqz(W4KG z$GV@?snl740jPQn1d5SUlPXNgeM6%+{)ic{tXnlr9fF+&;JL4<+Wn-KN6si)UJW^B zCl?ktspip!YUkA)YW4m9Cz*w<)h;%>Q&WVH%eJzq?Bi1yu$k!fwKshW* zr`*p6dFc2o>teIDpS9`P?p6$HcLET*Z=*#Tge%ECUVP_DuQrm7Pwsc4bO@9USSpx{1BQU zyB-=K;e|Rv`36-VBz@@gl!ttiD7{5N7x70RakwkpU;!_N@1d6W6$FYgqM0OxB+uQy zK{8I#+0CICpEYH@Bp&NVofaVY8z#<>a3)#JKD*f0zS1M+8s;kJ5Pxvk#TZyUE0 zvk8$DDbO6CdW;|j<;R=qm|SPDTPH97dQ+V=*U5M549OpDs*~Y5L!COl77Sy=Bfid9 z5BLWNCFkqyBw-<}CCX1>ede$-w#2DKkZYSFE0BlWt$sS#8f*zhcs&fXCN`{xK5v9| zeVI)%+sc~H6*nPo!l%#CTv!NzyLYUp(R#cCY$X^Ey$-auqzPdDyBe>l>AU)d6@!8& z(O%L<NQJm$C_ao_JBx|=C^7ucXz@rsnKqa;cT@h?vp1E`--bhyZ;CQFqPd02h zPva?5ut32g1>NDv3X=bXr#rBR_x3ERfk{uUXVO@#YQZpVOqtVSh5>eWbu^ZQ5u~JG{qTQ?tiItV z;r-}+g0XqNyF>b-yp}ceu#VHDgdaMdAFUrxZ!D3D|LL(vdNd6Gv&QnD>&LQ@H}`b7 z(^!xSnb4qFl;kuBw^hlb*k`qQYu~4*UZYOWQ$RMJZF6Lnx7j0v8Q3fX%tyXGdq+2q zjkD7$+14$APs4?KgPno)Bi-z%L4RSS5g%aRBPnO0Cn@iPr!*Wp-Rm{iPtP9QIH-}$ zorZ%w2*b<>hnYvBk4YQwK_S_<0Iw07HmpL{=c5l^T%Vx6M+{aHu>bc>jYGl$5h6fH z?!Qq^KkRMlgm?2DRISUefnJH^zw;YHq>FezMq(4<1)`PD1JB~kcotYUNXKY52edoD zXg3?9-Swrp80`Xq!1EHJ-O6UP8)OK(c93f@APo3GGVQbg3R%+_cMtj*@*xB{w$#bk zTXwdGT&>7f!=ZqEd*n9gKQJJ1dJv8cRY#JWsgUgxZWigDS|xsMz^Z)$KP1T`8_8sm zVxKRV@)p%2^UgR^&Qk25*m6r(Kq5~ieF{K?;+;SmF-UEFH5t^6SuVx}nel;!jtHGa$nM?D`!IKuia|ORz26F{Gh`o?5&!a5c z&e85>op)*gwKIL*sXV1YYrr35@07Zz$#23pH3VPWFe6KaeN)sb8yMJe(XI^pE1`Xq zTfwe|eW-;E_|2_a@m3W+(-Qdwim}r)a*^aDStZ`rMmyX|N*}3ip3QkinMZp;g&QkCy1?MO@i2(a3 zmMiX&`|jEC?f~T1sX7n(b&B;yK0(QG3TP)28O)y5B-!5C6QKufyVY^ft%J5CYNl9C3;5#^8cmBL`TfwG97e;5(5~z2S zW6IL5fL!B=mc!F(;dg<|5;+aYRFTW8r30BLA{^FZZ6Sv3#Bjj&FA>ci%L!+lC6*Io2xqkl z545p;c)bg%3MvZ+I#?#6tuE!Cp%^V#c@x2LB2NmP94V3GB!iA}I+vE{BRfCZG?L@v z6dv!}2zUu=bRe;!b6jWF#2zSXBX&(B;!`Ap7C)9CDFIVZm=;=D6Z!}GN}@FqB-$B2 zK@MtKws4pqhO0XZhxvyp9P_x{u>&VeIXXK6?E&ghRlX73+b~v=Ur%=KZGcXmYISP! z$#SA~gJ`5eg!^QQdl#g9wsBIVXET+OVt~sR=nwczekvYAA;SIIG2K;dJ$40ISMtm zYG==3#4;MPow|JT4d>#e89VpN)iY{uL%OOHGgvDtB)P%Z{!1Q9J@B!mtK9RxM40Lu9 zTAof~Gzb=aTaxU)2AhKTiU})_STrQ8!AfE&4=Q9^H0{j^7EK?I`klBwT(<=olf!IEOyw;O~Pa>wUxPE6E@U-+_UV z{M*te+(+kovYax4EWCL?Uiu`+0=XoA4(hF)UQh z0gy#09nn!~67&L}4kQ6w)-*aj_beZx*v?QH-RN10TH|OBZeMDZr4k765BwaoCAaf_ z@BkVbXh1Y08ZlkHKQFog(OQZHtx<+tMbgFBYC1DX@)apL61Fl9mF97?MAU10af61G z86=TO)SIzD;*w5O4fSL1m{poD*5+Kn6Fa zP8afh;HkISWZ`9a{)?EVFiDEATnP;zv}JjMv!Wvs`Ad{Y(Fc~Q4O>S5W%(<}WKj*1 z(7v2SsD?WsdXbuFP!k_!FNBCaag;bVZ&k(kx+)Om|%Z$3s5l zZy`(mHU*5dx^OET>eC;NSG?hVpXQ5rt|ZBpjc9TW@pWo|_IacVSPe7~iD2uB0q=Jr zFaT98EM?vcVW!xQ0i+lJJE>Jx5KBYQ>##5%X)9)257%s<0Mc;^Dxr-X3tg%~ZpKex zH-mi>UH%h*#RX#;?mK~=!(9K36W*alUK*1xLiDW-UJT^3kroAXND?_>va&#N+Kq<8 z>+lXCljvlGx{`lD0c}RnDIK_C$uM@w_?7NK-w@*KXyBBwLTBmGl3CAu`MV(YSH$lT zL=?gj;CLPwB7<;LxnA{g+ho(kOFW z2gg&swxAPOWsaM1ia3tTUn0RAPy2LQXJWoO98XSfg8T$*$dY;c5isQ6)A3M~9cb&( zH5%5=Y$M{0z;0A87=2U2d%hXE*cOdRP zXsaN9AB6hkCB^={NH+FT(MuGt(DXzl6XK{BI3u#+A5#rpYw3~*4w#KQ%Wa39gpucS2gyprgaS`g!o2?xP zqAO$;-LC76l!#(PzWg(M6Y9^(+Z1`4<%6(6tXHjlxDadAYn5dt08L?y+F1%3Pk44laA^(DSn~JZKfKsHn zT90n{fR1vC3>NAw@|?DE@vR7pR9a_jf7uxz>eX53Te~C7)4N*7mye^#=+g9M8!cN~ zx+bVs9h4eujasjPd1N_@LSQe6Yq$~8Al}gpzPKOar^SOJ2Su&JR%+P!G5|DCU1Iar zVF{1?yWQb$86U@dphLc-GtQTE1OpMzRLjo8NG8569)|AvM|HBl*UaV3g!2S3|S+dRy!4|2l+e5vQzPL=QS#6 zm&F1a*>*Dgefa3lDcMuj93#(BXda91Iv*!X<>jE*j3n*y`H#(04w{(|Q}o~Uz?`Aj zLlD=|>0|Mw=7hP^e9{~=$IQvhp3Hzr|3n5g$sXXoL^q_q z;otso64Y>j)`_8L2C#%E$Chl9=z$MF6yzz+<=FJeH{7@bqDg1 L8;!LS{`da@Ip82> literal 0 HcmV?d00001 diff --git a/px4_ros_com/scripts/__pycache__/uorb_rtps_classifier.cpython-38.pyc b/px4_ros_com/scripts/__pycache__/uorb_rtps_classifier.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..56eec502bfb06347dd824965989ec197df8bc39f GIT binary patch literal 6323 zcmb_g&2JmW72la%E>|=~$+l!mPAsoKVuguG<-;k^)Nta)X&N9tMU$EUA@9 zE3H*&(azO!ZL?;!^R+xnajly5nkIB%+|`7!#cLB@LFCr?uekh|rwtBmYqg?RT$>c8 z$O}tMh=MruagJ#URuq$>1lZIFY+4)^M*urI0-F)D;uv7ZM__YeUYr2zu@Ttg;-pyc zO5&8T#p#dDjLR8uR-6O$32}Z19ZX?g)86#j;o1>ug^e9EdCt?$PEz$MHJ3iA|j?JbsKd#5ovvjxqq z8^HXps3^>8M)S;&W`~QV5y}f0#b@rNNSwj|yc@=FWxwJ0=W~yKB^pYy=a=DCe1K4#td8J{!TT zF|smD7&b<51@Dl;Es3eS#ulp;1;-Yrlfo-4Hw^uT@5%W4s&7YuT~C1<_B+?#x@Nb- zW@x)1O@v-YT(G5A_x#(Q?REqpksYmizO-Mz8djIfEHR}Q^<<~6<0Qi$_33#9Pk0ke zpZ7IK>uUqnhkc=s@j&0!m=+mZfIGSDZ8m9?SFNSm31Aio6K{AmW{c=es$NNG*R&nB zT%Jk_*tXLMTEdgbOxKm6=WMv`mP4m;8h*=5@`TZLyUA32)vK>ND=u)Njjop%f)rL% zs8Qh1sgtSUC_8;}#BKR*n9>^}&5px{s1|W*k@_bguU?lHDb7bQR0G;jm@+ES-ui zjmAt?)Ggg&7JB&W7B8^)s4_2QYSrPIIwSn-hv2W7c$AU+6P>=c23MNbu4?apy2%8O zU@QZ^jX<<+%9}zT8~+g4`oQSJSLQXe`n=ZQ+Z3{9Fq0dY>5RzvFgUoVIkx(f1Jr(l zHTefpvkp=N_QVjIsNX7xJp9XwpS?!eE7E?Utz{aeKnscK@y&vr{jR8!DCu-v2o3RvB&A%#{$SOw zukK0Bb|pmU%5_fwZI8l=M-eNk?g7SjM)sD+bCh;wqoMHf{@f;u)UL2M9X6vK*S7hu z`7MJ2OKeo3iyb530aQf0N{qF@@1%al6JC8ro&u#ErbN6i)}(NWGzEw1UVe}Gkw{F& zcjITrrFbA3R4=uHy4wmbqcdDz*5zreRw|Zfs7J1s@=`seM>^FzctUb1EmvU0-T34~ zd5sC@QPuX-JsE#1YRl|gd|RrM=OF~SNX^sK>`P1us|uKdXJ0KBA1JD9YY)?mal=u|_NLj-4RU=mGS~8bv(M9F z`eP%Fska)D)z6`rnpijF-y$lp3j@?6v@>wcQMw)$UQ|wg#6%H3KN(kEp#*IE4O`{U zp>(dK^CeaoNz4Zl_BBTUgQ|f<2kfBuCop{nPg;`doE6y_)GtK{ z+}P#Fyata?jR^XFSA_B;mA4(Y?Kw_TaGZ7^dM)Y~9p_fhZDlig#}Pr@apYs*rwXXl znkfZKiuRIhAt{ED8~OFN7p(?D770$99ND-EIw~PiNFx&tsdTfL!Amox!zByG%;4a zRu`1qIA5tK??`g$6;BfZ)y>ENOU4tGN|p$v4VLlzJ#2<49O!;*RVse7gH0u7`i|%B zS&&W$;CJabR9(q3(LP3~sJmcWxN-VXXwzH>#CAMYNds5~^A+LC#6$=0`0Vxc%voU# z8xq#EGE4c3;`GDVOfj4T!3(c7Oq+9inStr(S!ok5vt@X z@xoWk7fJnTar~kk^9%M^sGu}S$)k=(c?1$ii7H*n^;vR2Xpod#b{GP0BzMwun5fIE zI{0`_RAKxNL{Eirc$9+3=ZOvFwc+nkc*}RODlO6O!)*tXd)(rDW|d!u*}?pe!_gL( zQ|D79;eGghGRjY0#OUkry01_ley~4{GK!}%*BNT_8#28C;v+#u#TQD!e;#G}4S76B zf`55b9tRaAsZxHpmj3#vJpK;_{e6^2c`>nE*;E-rewX%lg&LKiOhggC)0AZNk~~h5 zcgUBhp|CAqLxXBXHhXPcO;yfB&`N0~MOUCU$>tqt8#t_ph}l%RaB_&Rjg^c$aN(WwVa{ty-mc;Fp!a#engNj z5}3X%s6a323s3qjL48%&_ZrnB<4UYcX`;M*2BYv(H1sKm{xi(dOFt