From 3a4dad655a45af93805f566c718abb8df37088eb Mon Sep 17 00:00:00 2001 From: Claudio Fritsche Date: Fri, 18 Nov 2022 11:32:51 +0100 Subject: [PATCH 1/4] Move old px4-offboard to separate folder and create new cpp package --- offboard_cpp/CMakeLists.txt | 34 +++++++++++++++++++ offboard_cpp/package.xml | 18 ++++++++++ offboard_cpp/src/offboard_cpp_node.cpp | 10 ++++++ .../offboard_position_control.launch.py | 0 .../launch}/visualize.launch.py | 0 package.xml => offboard_python/package.xml | 0 .../px4_offboard}/__init__.py | 0 .../px4_offboard}/offboard_control.py | 0 .../px4_offboard}/visualizer.py | 0 .../resource}/px4_offboard | 0 .../resource}/visualize.rviz | 0 setup.cfg => offboard_python/setup.cfg | 0 setup.py => offboard_python/setup.py | 0 .../test}/test_copyright.py | 0 {test => offboard_python/test}/test_flake8.py | 0 {test => offboard_python/test}/test_pep257.py | 0 16 files changed, 62 insertions(+) create mode 100644 offboard_cpp/CMakeLists.txt create mode 100644 offboard_cpp/package.xml create mode 100644 offboard_cpp/src/offboard_cpp_node.cpp rename {launch => offboard_python/launch}/offboard_position_control.launch.py (100%) rename {launch => offboard_python/launch}/visualize.launch.py (100%) rename package.xml => offboard_python/package.xml (100%) rename {px4_offboard => offboard_python/px4_offboard}/__init__.py (100%) rename {px4_offboard => offboard_python/px4_offboard}/offboard_control.py (100%) rename {px4_offboard => offboard_python/px4_offboard}/visualizer.py (100%) rename {resource => offboard_python/resource}/px4_offboard (100%) rename {resource => offboard_python/resource}/visualize.rviz (100%) rename setup.cfg => offboard_python/setup.cfg (100%) rename setup.py => offboard_python/setup.py (100%) rename {test => offboard_python/test}/test_copyright.py (100%) rename {test => offboard_python/test}/test_flake8.py (100%) rename {test => offboard_python/test}/test_pep257.py (100%) diff --git a/offboard_cpp/CMakeLists.txt b/offboard_cpp/CMakeLists.txt new file mode 100644 index 0000000..294f649 --- /dev/null +++ b/offboard_cpp/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) +project(offboard_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(offboard_cpp_node src/offboard_cpp_node.cpp) +target_include_directories(offboard_cpp_node PUBLIC + $ + $) +target_compile_features(offboard_cpp_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS offboard_cpp_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/offboard_cpp/package.xml b/offboard_cpp/package.xml new file mode 100644 index 0000000..caf6153 --- /dev/null +++ b/offboard_cpp/package.xml @@ -0,0 +1,18 @@ + + + + offboard_cpp + 0.0.0 + Simple PX4 offboard example with ROS2 and C++ + claudio + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/offboard_cpp/src/offboard_cpp_node.cpp b/offboard_cpp/src/offboard_cpp_node.cpp new file mode 100644 index 0000000..348ad5a --- /dev/null +++ b/offboard_cpp/src/offboard_cpp_node.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world offboard_cpp package\n"); + return 0; +} diff --git a/launch/offboard_position_control.launch.py b/offboard_python/launch/offboard_position_control.launch.py similarity index 100% rename from launch/offboard_position_control.launch.py rename to offboard_python/launch/offboard_position_control.launch.py diff --git a/launch/visualize.launch.py b/offboard_python/launch/visualize.launch.py similarity index 100% rename from launch/visualize.launch.py rename to offboard_python/launch/visualize.launch.py diff --git a/package.xml b/offboard_python/package.xml similarity index 100% rename from package.xml rename to offboard_python/package.xml diff --git a/px4_offboard/__init__.py b/offboard_python/px4_offboard/__init__.py similarity index 100% rename from px4_offboard/__init__.py rename to offboard_python/px4_offboard/__init__.py diff --git a/px4_offboard/offboard_control.py b/offboard_python/px4_offboard/offboard_control.py similarity index 100% rename from px4_offboard/offboard_control.py rename to offboard_python/px4_offboard/offboard_control.py diff --git a/px4_offboard/visualizer.py b/offboard_python/px4_offboard/visualizer.py similarity index 100% rename from px4_offboard/visualizer.py rename to offboard_python/px4_offboard/visualizer.py diff --git a/resource/px4_offboard b/offboard_python/resource/px4_offboard similarity index 100% rename from resource/px4_offboard rename to offboard_python/resource/px4_offboard diff --git a/resource/visualize.rviz b/offboard_python/resource/visualize.rviz similarity index 100% rename from resource/visualize.rviz rename to offboard_python/resource/visualize.rviz diff --git a/setup.cfg b/offboard_python/setup.cfg similarity index 100% rename from setup.cfg rename to offboard_python/setup.cfg diff --git a/setup.py b/offboard_python/setup.py similarity index 100% rename from setup.py rename to offboard_python/setup.py diff --git a/test/test_copyright.py b/offboard_python/test/test_copyright.py similarity index 100% rename from test/test_copyright.py rename to offboard_python/test/test_copyright.py diff --git a/test/test_flake8.py b/offboard_python/test/test_flake8.py similarity index 100% rename from test/test_flake8.py rename to offboard_python/test/test_flake8.py diff --git a/test/test_pep257.py b/offboard_python/test/test_pep257.py similarity index 100% rename from test/test_pep257.py rename to offboard_python/test/test_pep257.py From 7d988e6c4fe0b4a15d5c4a21b6931329a05b5bb4 Mon Sep 17 00:00:00 2001 From: Claudio Fritsche Date: Fri, 18 Nov 2022 11:34:41 +0100 Subject: [PATCH 2/4] Modify offboard code from px4_ros_com to work with the new dds system --- offboard_cpp/CMakeLists.txt | 5 + offboard_cpp/package.xml | 5 + offboard_cpp/src/offboard_cpp_node.cpp | 207 ++++++++++++++++++++++++- 3 files changed, 212 insertions(+), 5 deletions(-) diff --git a/offboard_cpp/CMakeLists.txt b/offboard_cpp/CMakeLists.txt index 294f649..09a49d6 100644 --- a/offboard_cpp/CMakeLists.txt +++ b/offboard_cpp/CMakeLists.txt @@ -10,8 +10,13 @@ find_package(ament_cmake REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(px4_ros_com REQUIRED) add_executable(offboard_cpp_node src/offboard_cpp_node.cpp) +ament_target_dependencies(offboard_node rclcpp std_msgs px4_msgs px4_ros_com) target_include_directories(offboard_cpp_node PUBLIC $ $) diff --git a/offboard_cpp/package.xml b/offboard_cpp/package.xml index caf6153..5978d84 100644 --- a/offboard_cpp/package.xml +++ b/offboard_cpp/package.xml @@ -12,6 +12,11 @@ ament_lint_auto ament_lint_common + rclcpp + std_msgs + px4_msgs + px4_ros_com + ament_cmake diff --git a/offboard_cpp/src/offboard_cpp_node.cpp b/offboard_cpp/src/offboard_cpp_node.cpp index 348ad5a..5815088 100644 --- a/offboard_cpp/src/offboard_cpp_node.cpp +++ b/offboard_cpp/src/offboard_cpp_node.cpp @@ -1,10 +1,207 @@ -#include +#include +#include +#include +#include +#include + +#include +#include + +// #include + +using namespace std::chrono; +using namespace std::chrono_literals; + +using std::placeholders::_1; + + +class OffboardControl : public rclcpp::Node +{ +public: + explicit OffboardControl() : Node("offboard_control") + { + auto qos_sub = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort().durability_volatile(); + auto qos_pub = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort().transient_local(); + + + // Create subscribers + this->vehicle_status_sub_ = this->create_subscription("/fmu/out/vehicle_status", + qos_sub, std::bind(&OffboardControl::vehicle_status_clbk, this, _1) + ); + + // // get common timestamp + // this->timesync_sub_ = + // this->create_subscription("fmu/timesync/out", qos, + // [this](const px4_msgs::msg::Timesync::UniquePtr msg) { + // timestamp_.store(msg->timestamp); + // }); + + // Create publishers + this->offboard_control_mode_publisher_ = + this->create_publisher("/fmu/in/offboard_control_mode", qos_pub); + this->trajectory_setpoint_publisher_ = + this->create_publisher("/fmu/in/trajectory_setpoint", qos_pub); + this->vehicle_command_publisher_ = + this->create_publisher("/fmu/in/vehicle_command", qos_pub); + + // Set default variables: + this->offboard_setpoint_counter_ = 0; + + // timer callback + auto timer_callback = [this]() -> void { + + if (this->offboard_setpoint_counter_ == 10) { + // Change to Offboard mode after 10 setpoints + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + + // Arm the vehicle + this->arm(); + } + + // offboard_control_mode needs to be paired with trajectory_setpoint + this->publish_offboard_control_mode(); + this->publish_trajectory_setpoint(); + + // stop the counter after reaching 11 + if (this->offboard_setpoint_counter_ < 11) { + this->offboard_setpoint_counter_++; + } + }; + + // start publisher timer + timer_ = this->create_wall_timer(100ms, timer_callback); + + } + + void arm() ; + void disarm() ; + + +private: + + // important node variables + rclcpp::TimerBase::SharedPtr timer_; // Timer that makes the node spin + uint8_t nav_state; + uint8_t arming_state; + // std::atomic timestamp_; //!< common synced timestamped + uint64_t offboard_setpoint_counter_; //!< counter for the number of setpoints sent + + + + // Subscribers + rclcpp::Subscription::SharedPtr vehicle_status_sub_; + // rclcpp::Subscription::SharedPtr timesync_sub_; + + + // publishers + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; + rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + + // callbacks + + void vehicle_status_clbk(const px4_msgs::msg::VehicleStatus & msg) + { + this->arming_state = msg.arming_state; + this->nav_state = msg.nav_state; + RCLCPP_INFO(this->get_logger(), "ArmingState: %i\nNavState: %i", this->arming_state, this->nav_state); + } + + // commands + void publish_offboard_control_mode(); + void publish_trajectory_setpoint() ; + void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0) ; + +}; + +/** + * @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 OffboardControl::publish_vehicle_command(uint16_t command, float param1, + float param2) { + px4_msgs::msg::VehicleCommand msg{}; + msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000); + msg.param1 = param1; + msg.param2 = param2; + 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); +} + + + +/** + * @brief Send a command to Arm the vehicle + */ +void OffboardControl::arm() { + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); + + RCLCPP_INFO(this->get_logger(), "Arm command send"); +} + +/** + * @brief Send a command to Disarm the vehicle + */ +void OffboardControl::disarm() { + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); + + RCLCPP_INFO(this->get_logger(), "Disarm command send"); +} + +/** + * @brief Publish the offboard control mode. + * For this example, only position and altitude controls are active. + */ +void OffboardControl::publish_offboard_control_mode() { + px4_msgs::msg::OffboardControlMode msg{}; + msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000); + msg.position = true; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = false; + msg.body_rate = false; + + offboard_control_mode_publisher_->publish(msg); +} + + +/** + * @brief Publish a trajectory setpoint + * For this example, it sends a trajectory setpoint to make the + * vehicle hover at 5 meters with a yaw angle of 180 degrees. + */ +void OffboardControl::publish_trajectory_setpoint() { + px4_msgs::msg::TrajectorySetpoint msg{}; + msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000); + msg.position = {0.0, 0.0, -5.0}; + msg.yaw = -3.14; // [-PI:PI] + + trajectory_setpoint_publisher_->publish(msg); +} + +/** + * @brief Entry point of Offboard node + * + * @param argc + * @param argv + * @return int + */ int main(int argc, char ** argv) { - (void) argc; - (void) argv; + std::cout << "Starting offboard_control node..." << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); - printf("hello world offboard_cpp package\n"); - return 0; + rclcpp::shutdown(); + return 0; } From 83e5d7d24b4c3e2e12283fa2e973671771667acc Mon Sep 17 00:00:00 2001 From: Claudio Fritsche Date: Fri, 18 Nov 2022 12:14:27 +0100 Subject: [PATCH 3/4] Offboard mode works. Added readme to explain procedure and fixed small typo in CMakeLists.txt --- .gitignore | 8 +++++ offboard_cpp/CMakeLists.txt | 2 +- offboard_cpp/README.md | 66 +++++++++++++++++++++++++++++++++++++ offboard_cpp/package.xml | 2 +- 4 files changed, 76 insertions(+), 2 deletions(-) create mode 100644 offboard_cpp/README.md diff --git a/.gitignore b/.gitignore index cceaafc..b76597a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,10 @@ *.pyc .vscode/* + + +px4_msgs/* +px4_ros_com/* + +build/* +install/* +log/* \ No newline at end of file diff --git a/offboard_cpp/CMakeLists.txt b/offboard_cpp/CMakeLists.txt index 09a49d6..1d7bada 100644 --- a/offboard_cpp/CMakeLists.txt +++ b/offboard_cpp/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(px4_msgs REQUIRED) find_package(px4_ros_com REQUIRED) add_executable(offboard_cpp_node src/offboard_cpp_node.cpp) -ament_target_dependencies(offboard_node rclcpp std_msgs px4_msgs px4_ros_com) +ament_target_dependencies(offboard_cpp_node rclcpp std_msgs px4_msgs px4_ros_com) target_include_directories(offboard_cpp_node PUBLIC $ $) diff --git a/offboard_cpp/README.md b/offboard_cpp/README.md new file mode 100644 index 0000000..b1bbbc9 --- /dev/null +++ b/offboard_cpp/README.md @@ -0,0 +1,66 @@ +# offboard_cpp +This `package` contains a c++ example for offboard control on ROS2 with [PX4](https://px4.io/).The source code is released under a BSD 3-Clause license. + + +## Instructions + +### Setup +Add this `repository` to your RO2 workspace. + ``` +git clone https://github.com/Jaeyoung-Lim/px4-offboard.git +``` + +Make sure that the newest versions of [px4_msgs](https://github.com/PX4/px4_msgs) and [px4_ros_com](https://github.com/PX4/px4_ros_com) are in the same workspace. + + ``` +git clone --recursive https://github.com/PX4/px4_ros_com.git +git clone --recursive https://github.com/PX4/px4_msgs.git +``` + +In a different folder, outside of your ros2 workspace clone the [PX4-Autopilot](https://github.com/PX4/PX4-Autopilot). At the time of writing this was commit [f2607335ac](https://github.com/PX4/PX4-Autopilot/commit/f2607335ac80dbd0fe151ba8fde122e91d776fc7). + + ``` +git clone --recursive https://github.com/PX4/PX4-Autopilot.git + +``` + +Finally you need to make sure that you have installed the `micro-ros-agent`. You can use `snap` to install it or [build it from source](https://github.com/micro-ROS/micro_ros_setup#building-micro-ros-agent). + +### Build +1. Go to your ros2 workspace +2. Source your ros2 installation. For me this was: +``` +source /opt/ros/galactic/setup.zsh +``` +3. Build your workspace, which includes the 4 packages (px4_msgs, px4_ros_com and the two offboard packages from this repo.. +``` +colcon build --symlink-install +``` +### Run +1. Open a new terminal and go to your PX4-Autopilot folder. Run: +``` +make px4_sitl gazebo +``` + +2. Open a new terminal and source your ros2 workspace. Then run the micro-ros-agent: +``` +micro-ros-agent udp4 --port 8888 +``` + +3. Open a new terminal and source your ros2 workspace. Then run the c++ offboard node: +``` +ros2 run offboard_cpp offboard_cpp_node +``` + +4. The SITL drone should have taken-off. + +### Troubleshoot +* I don't know why but sometimes the offboard node does not manage to change the `NavState` of the SITL drone to offboard (14) and it remains on hold mode (4). To change that use QGC or use your nutt-shell: +``` +commander mode offboard +``` +* Once the offboard mode is set the SITL drone must be armed using QGC or the nutt-shell: +``` +commander arm +``` + diff --git a/offboard_cpp/package.xml b/offboard_cpp/package.xml index 5978d84..91fe4ef 100644 --- a/offboard_cpp/package.xml +++ b/offboard_cpp/package.xml @@ -5,7 +5,7 @@ 0.0.0 Simple PX4 offboard example with ROS2 and C++ claudio - TODO: License declaration + BSD 3-Clause ament_cmake From 90e5af9241d90a21b0ff76dc9f5c38fe645f27ce Mon Sep 17 00:00:00 2001 From: Claudio Date: Tue, 22 Nov 2022 12:29:20 +0100 Subject: [PATCH 4/4] Update offboard Node to use M_PI instead of 3.14 Co-authored-by: JaeyoungLim --- offboard_cpp/src/offboard_cpp_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/offboard_cpp/src/offboard_cpp_node.cpp b/offboard_cpp/src/offboard_cpp_node.cpp index 5815088..b2cec18 100644 --- a/offboard_cpp/src/offboard_cpp_node.cpp +++ b/offboard_cpp/src/offboard_cpp_node.cpp @@ -183,7 +183,7 @@ void OffboardControl::publish_trajectory_setpoint() { px4_msgs::msg::TrajectorySetpoint msg{}; msg.timestamp = int(this->get_clock()->now().nanoseconds() / 1000); msg.position = {0.0, 0.0, -5.0}; - msg.yaw = -3.14; // [-PI:PI] + msg.yaw = -M_PI; // [-PI:PI] trajectory_setpoint_publisher_->publish(msg); }