From 2d56cb6e0aa598d576f0d8ac39062ab9bdc5f3f1 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sun, 5 Nov 2023 14:16:28 +0000 Subject: [PATCH] Add example that uses VehicleCommand service Signed-off-by: Beniamino Pozzan --- CMakeLists.txt | 5 + package.xml | 3 +- .../offboard/offboard_control_srv.cpp | 289 ++++++++++++++++++ 3 files changed, 296 insertions(+), 1 deletion(-) create mode 100644 src/examples/offboard/offboard_control_srv.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 396187d1..1a3b45fa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,11 @@ add_executable(offboard_control src/examples/offboard/offboard_control.cpp) ament_target_dependencies(offboard_control rclcpp px4_msgs) install(TARGETS offboard_control DESTINATION lib/${PROJECT_NAME}) +# examples/offboard/offboard_control_srv +add_executable(offboard_control_srv src/examples/offboard/offboard_control_srv.cpp) +ament_target_dependencies(offboard_control_srv rclcpp px4_msgs) +install(TARGETS offboard_control_srv DESTINATION lib/${PROJECT_NAME}) + ############ # Install ## diff --git a/package.xml b/package.xml index ff1f10ec..529d1ef2 100644 --- a/package.xml +++ b/package.xml @@ -5,7 +5,8 @@ 0.1.0 Package to interface ROS 2 with PX4 - Nuno Marques + Beniamino Pozzan + Beniamino Pozzan Nuno Marques BSD 3-Clause diff --git a/src/examples/offboard/offboard_control_srv.cpp b/src/examples/offboard/offboard_control_srv.cpp new file mode 100644 index 00000000..a7f9c405 --- /dev/null +++ b/src/examples/offboard/offboard_control_srv.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright 2023 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 Offboard control example + * @file offboard_control.cpp + * @addtogroup examples * + * @author Beniamino Pozzan + * @author Mickey Cowden + * @author Nuno Marques + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std::chrono; +using namespace std::chrono_literals; +using namespace px4_msgs::msg; + +class OffboardControl : public rclcpp::Node +{ +public: + OffboardControl(std::string px4_namespace) : + Node("offboard_control_srv"), + state_{State::init}, + service_result_{0}, + service_done_{false}, + offboard_control_mode_publisher_{this->create_publisher(px4_namespace+"in/offboard_control_mode", 10)}, + trajectory_setpoint_publisher_{this->create_publisher(px4_namespace+"in/trajectory_setpoint", 10)}, + vehicle_command_client_{this->create_client(px4_namespace+"vehicle_command")} + { + RCLCPP_INFO(this->get_logger(), "Starting Offboard Control example with PX4 services"); + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for " << px4_namespace << "vehicle_command service"); + while (!vehicle_command_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + timer_ = this->create_wall_timer(100ms, std::bind(&OffboardControl::timer_callback, this)); + } + + void switch_to_offboard_mode(); + void arm(); + void disarm(); + +private: + enum class State{ + init, + offboard_requested, + wait_for_stable_offboard_mode, + arm_requested, + armed + } state_; + uint8_t service_result_; + bool service_done_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher_; + rclcpp::Client::SharedPtr vehicle_command_client_; + + + void publish_offboard_control_mode(); + void publish_trajectory_setpoint(); + void request_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0); + void response_callback(rclcpp::Client::SharedFuture future); + void timer_callback(void); +}; + +/** + * @brief Send a command to switch to offboard mode + */ +void OffboardControl::switch_to_offboard_mode(){ + RCLCPP_INFO(this->get_logger(), "requesting switch to Offboard mode"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); +} + +/** + * @brief Send a command to Arm the vehicle + */ +void OffboardControl::arm() +{ + RCLCPP_INFO(this->get_logger(), "requesting arm"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0); +} + +/** + * @brief Send a command to Disarm the vehicle + */ +void OffboardControl::disarm() +{ + RCLCPP_INFO(this->get_logger(), "requesting disarm"); + request_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0); +} + +/** + * @brief Publish the offboard control mode. + * For this example, only position and altitude controls are active. + */ +void OffboardControl::publish_offboard_control_mode() +{ + OffboardControlMode msg{}; + msg.position = true; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = false; + msg.body_rate = false; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + 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() +{ + TrajectorySetpoint msg{}; + msg.position = {0.0, 0.0, -5.0}; + msg.yaw = -3.14; // [-PI:PI] + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + trajectory_setpoint_publisher_->publish(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 OffboardControl::request_vehicle_command(uint16_t command, float param1, float param2) +{ + auto request = std::make_shared(); + + VehicleCommand msg{}; + 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; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + request->request = msg; + + service_done_ = false; + auto result = vehicle_command_client_->async_send_request(request, std::bind(&OffboardControl::response_callback, this, + std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Command send"); +} + +void OffboardControl::timer_callback(void){ + static uint8_t num_of_steps = 0; + + // offboard_control_mode needs to be paired with trajectory_setpoint + publish_offboard_control_mode(); + publish_trajectory_setpoint(); + + switch (state_) + { + case State::init : + switch_to_offboard_mode(); + state_ = State::offboard_requested; + break; + case State::offboard_requested : + if(service_done_){ + if (service_result_==0){ + RCLCPP_INFO(this->get_logger(), "Entered offboard mode"); + state_ = State::wait_for_stable_offboard_mode; + } + else{ + RCLCPP_ERROR(this->get_logger(), "Failed to enter offboard mode, exiting"); + rclcpp::shutdown(); + } + } + break; + case State::wait_for_stable_offboard_mode : + if (++num_of_steps>10){ + arm(); + state_ = State::arm_requested; + } + break; + case State::arm_requested : + if(service_done_){ + if (service_result_==0){ + RCLCPP_INFO(this->get_logger(), "vehicle is armed"); + state_ = State::armed; + } + else{ + RCLCPP_ERROR(this->get_logger(), "Failed to arm, exiting"); + rclcpp::shutdown(); + } + } + break; + default: + break; + } +} + +void OffboardControl::response_callback( + rclcpp::Client::SharedFuture future) { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) { + auto reply = future.get()->reply; + service_result_ = reply.result; + switch (service_result_) + { + case reply.VEHICLE_CMD_RESULT_ACCEPTED: + RCLCPP_INFO(this->get_logger(), "command accepted"); + break; + case reply.VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + RCLCPP_WARN(this->get_logger(), "command temporarily rejected"); + break; + case reply.VEHICLE_CMD_RESULT_DENIED: + RCLCPP_WARN(this->get_logger(), "command denied"); + break; + case reply.VEHICLE_CMD_RESULT_UNSUPPORTED: + RCLCPP_WARN(this->get_logger(), "command unsupported"); + break; + case reply.VEHICLE_CMD_RESULT_FAILED: + RCLCPP_WARN(this->get_logger(), "command failed"); + break; + case reply.VEHICLE_CMD_RESULT_IN_PROGRESS: + RCLCPP_WARN(this->get_logger(), "command in progress"); + break; + case reply.VEHICLE_CMD_RESULT_CANCELLED: + RCLCPP_WARN(this->get_logger(), "command cancelled"); + break; + default: + RCLCPP_WARN(this->get_logger(), "command reply unknown"); + break; + } + service_done_ = true; + } else { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + +int main(int argc, char *argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("/fmu/")); + + rclcpp::shutdown(); + return 0; +}