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 00000000..746aadfa Binary files /dev/null and b/px4_ros_com/scripts/__pycache__/px_generate_uorb_topic_files.cpython-38.pyc differ 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 00000000..56eec502 Binary files /dev/null and b/px4_ros_com/scripts/__pycache__/uorb_rtps_classifier.cpython-38.pyc differ diff --git a/scripts/build_all.bash b/px4_ros_com/scripts/build_all.bash similarity index 100% rename from scripts/build_all.bash rename to px4_ros_com/scripts/build_all.bash diff --git a/scripts/build_ros1_bridge.bash b/px4_ros_com/scripts/build_ros1_bridge.bash similarity index 100% rename from scripts/build_ros1_bridge.bash rename to px4_ros_com/scripts/build_ros1_bridge.bash diff --git a/scripts/build_ros2_workspace.bash b/px4_ros_com/scripts/build_ros2_workspace.bash similarity index 100% rename from scripts/build_ros2_workspace.bash rename to px4_ros_com/scripts/build_ros2_workspace.bash diff --git a/scripts/clean_all.bash b/px4_ros_com/scripts/clean_all.bash similarity index 100% rename from scripts/clean_all.bash rename to px4_ros_com/scripts/clean_all.bash diff --git a/scripts/generate_microRTPS_bridge.py b/px4_ros_com/scripts/generate_microRTPS_bridge.py similarity index 100% rename from scripts/generate_microRTPS_bridge.py rename to px4_ros_com/scripts/generate_microRTPS_bridge.py diff --git a/scripts/px_generate_uorb_topic_files.py b/px4_ros_com/scripts/px_generate_uorb_topic_files.py similarity index 100% rename from scripts/px_generate_uorb_topic_files.py rename to px4_ros_com/scripts/px_generate_uorb_topic_files.py diff --git a/scripts/px_generate_uorb_topic_helper.py b/px4_ros_com/scripts/px_generate_uorb_topic_helper.py similarity index 100% rename from scripts/px_generate_uorb_topic_helper.py rename to px4_ros_com/scripts/px_generate_uorb_topic_helper.py diff --git a/scripts/setup_system.bash b/px4_ros_com/scripts/setup_system.bash similarity index 100% rename from scripts/setup_system.bash rename to px4_ros_com/scripts/setup_system.bash diff --git a/scripts/uorb_rtps_classifier.py b/px4_ros_com/scripts/uorb_rtps_classifier.py similarity index 100% rename from scripts/uorb_rtps_classifier.py rename to px4_ros_com/scripts/uorb_rtps_classifier.py diff --git a/src/lib/frame_transforms.cpp b/px4_ros_com/src/lib/frame_transforms.cpp similarity index 100% rename from src/lib/frame_transforms.cpp rename to px4_ros_com/src/lib/frame_transforms.cpp diff --git a/templates/Publisher.cpp.em b/px4_ros_com/templates/Publisher.cpp.em similarity index 100% rename from templates/Publisher.cpp.em rename to px4_ros_com/templates/Publisher.cpp.em diff --git a/templates/Publisher.h.em b/px4_ros_com/templates/Publisher.h.em similarity index 100% rename from templates/Publisher.h.em rename to px4_ros_com/templates/Publisher.h.em diff --git a/templates/RtpsTopics.cpp.em b/px4_ros_com/templates/RtpsTopics.cpp.em similarity index 100% rename from templates/RtpsTopics.cpp.em rename to px4_ros_com/templates/RtpsTopics.cpp.em diff --git a/templates/RtpsTopics.h.em b/px4_ros_com/templates/RtpsTopics.h.em similarity index 100% rename from templates/RtpsTopics.h.em rename to px4_ros_com/templates/RtpsTopics.h.em diff --git a/templates/Subscriber.cpp.em b/px4_ros_com/templates/Subscriber.cpp.em similarity index 100% rename from templates/Subscriber.cpp.em rename to px4_ros_com/templates/Subscriber.cpp.em diff --git a/templates/Subscriber.h.em b/px4_ros_com/templates/Subscriber.h.em similarity index 100% rename from templates/Subscriber.h.em rename to px4_ros_com/templates/Subscriber.h.em diff --git a/templates/microRTPS_agent.cpp.em b/px4_ros_com/templates/microRTPS_agent.cpp.em similarity index 100% rename from templates/microRTPS_agent.cpp.em rename to px4_ros_com/templates/microRTPS_agent.cpp.em diff --git a/templates/microRTPS_timesync.cpp.em b/px4_ros_com/templates/microRTPS_timesync.cpp.em similarity index 100% rename from templates/microRTPS_timesync.cpp.em rename to px4_ros_com/templates/microRTPS_timesync.cpp.em diff --git a/templates/microRTPS_timesync.h.em b/px4_ros_com/templates/microRTPS_timesync.h.em similarity index 100% rename from templates/microRTPS_timesync.h.em rename to px4_ros_com/templates/microRTPS_timesync.h.em diff --git a/templates/microRTPS_transport.cpp b/px4_ros_com/templates/microRTPS_transport.cpp similarity index 100% rename from templates/microRTPS_transport.cpp rename to px4_ros_com/templates/microRTPS_transport.cpp diff --git a/templates/microRTPS_transport.h b/px4_ros_com/templates/microRTPS_transport.h similarity index 100% rename from templates/microRTPS_transport.h rename to px4_ros_com/templates/microRTPS_transport.h diff --git a/templates/urtps_bridge_topics.yaml b/px4_ros_com/templates/urtps_bridge_topics.yaml similarity index 100% rename from templates/urtps_bridge_topics.yaml rename to px4_ros_com/templates/urtps_bridge_topics.yaml diff --git a/test/__init__.py b/px4_ros_com/test/__init__.py similarity index 100% rename from test/__init__.py rename to px4_ros_com/test/__init__.py diff --git a/test/pipeline_io_test.py b/px4_ros_com/test/pipeline_io_test.py similarity index 100% rename from test/pipeline_io_test.py rename to px4_ros_com/test/pipeline_io_test.py diff --git a/test/test_input.py b/px4_ros_com/test/test_input.py similarity index 100% rename from test/test_input.py rename to px4_ros_com/test/test_input.py diff --git a/test/test_output.py b/px4_ros_com/test/test_output.py similarity index 100% rename from test/test_output.py rename to px4_ros_com/test/test_output.py