diff --git a/examples/cpp/modes/goto/include/mode.hpp b/examples/cpp/modes/goto/include/mode.hpp index 049ef8fc..ed7570a9 100644 --- a/examples/cpp/modes/goto/include/mode.hpp +++ b/examples/cpp/modes/goto/include/mode.hpp @@ -125,7 +125,7 @@ class FlightModeTest : public px4_ros2::ModeBase break; case State::GoingSouthwest: { - // go to southwest corner while facing the northeastern corner + // go to southwest corner while facing the northwestern corner const Eigen::Vector2f position_of_interest_m = _start_position_m.head(2) + Eigen::Vector2f{kTriangleHeight, 0.f}; const Eigen::Vector2f vehicle_to_poi_xy = position_of_interest_m - diff --git a/examples/cpp/modes/goto_global/CMakeLists.txt b/examples/cpp/modes/goto_global/CMakeLists.txt new file mode 100644 index 00000000..34a3e8e9 --- /dev/null +++ b/examples/cpp/modes/goto_global/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(example_mode_goto_global_cpp) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter) +endif() + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_ros2_cpp REQUIRED) + +include_directories(include ${Eigen3_INCLUDE_DIRS}) +add_executable(example_mode_goto_global + src/main.cpp) +ament_target_dependencies(example_mode_goto_global Eigen3 px4_ros2_cpp rclcpp) + +install(TARGETS + example_mode_goto_global + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/examples/cpp/modes/goto_global/include/mode.hpp b/examples/cpp/modes/goto_global/include/mode.hpp new file mode 100644 index 00000000..8b9833ef --- /dev/null +++ b/examples/cpp/modes/goto_global/include/mode.hpp @@ -0,0 +1,224 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +static const std::string kName = "Go-to Global Example"; +static const std::string kNodeName = "example_mode_goto_global"; + +using namespace px4_ros2::literals; // NOLINT + +class FlightModeTest : public px4_ros2::ModeBase +{ +public: + explicit FlightModeTest(rclcpp::Node & node) + : ModeBase(node, kName) + { + _goto_setpoint = std::make_shared(*this); + + _vehicle_global_position = std::make_shared(*this); + + _vehicle_attitude_sub = node.create_subscription( + "/fmu/out/vehicle_attitude", rclcpp::QoS(1).best_effort(), + [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) { + updateVehicleHeading(msg); + } + ); + } + + void onActivate() override + { + _state = State::SettlingAtStart; + _start_position_set = false; + _start_heading_set = false; + } + + void onDeactivate() override {} + + void updateSetpoint(float dt_s) override + { + if (!_start_position_set) { + _start_position_m = _vehicle_global_position->position(); + _start_position_set = true; + } + + switch (_state) { + case State::SettlingAtStart: { + // just settling at the starting vehicle position + _goto_setpoint->update(_start_position_m); + if (positionReached(_start_position_m)) { + _state = State::GoingNorth; + } + } + break; + + case State::GoingNorth: { + // go north to the northwest corner facing in direction of travel + const Eigen::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m, Eigen::Vector3f{kTriangleHeight, 0.f, 0.f}); + + const float heading_target_rad = px4_ros2::headingToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + + if (px4_ros2::horizontalDistanceToGlobalPosition( + _vehicle_global_position->position(), + target_position_m) < 0.1f) + { + // stop caring about heading (the arctangent becomes undefined) + _goto_setpoint->update(target_position_m); + } else { + _goto_setpoint->update(target_position_m, heading_target_rad); + } + + if (positionReached(target_position_m)) { + _state = State::GoingEast; + } + } + break; + + case State::GoingEast: { + // go to the northeast corner while spinning + const Eigen::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m, Eigen::Vector3f{kTriangleHeight, kTriangleWidth, 0.f}); + + // scale the speed limits by distance to the target + const float distance_to_target = px4_ros2::horizontalDistanceToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + const float speed_scale = std::min(distance_to_target / kTriangleWidth, 1.f); + + const float max_horizontal_velocity_m_s = 5.f * speed_scale + (1.f - speed_scale) * 1.f; + const float max_vertical_velocity_m_s = 3.f * speed_scale + (1.f - speed_scale) * 0.5f; + const float max_heading_rate_rad_s = + px4_ros2::degToRad(45.f * speed_scale + (1.f - speed_scale) * 25.f); + const float heading_setpoint_rate_of_change = + px4_ros2::degToRad(40.f * speed_scale + (1.f - speed_scale) * 20.f); + + if (!_start_heading_set) { + _spinning_heading_rad = _vehicle_heading_rad; + _start_heading_set = true; + } + + if (!positionReached(target_position_m)) { + _spinning_heading_rad += heading_setpoint_rate_of_change * dt_s; + } + + _goto_setpoint->update( + target_position_m, + _spinning_heading_rad, + max_horizontal_velocity_m_s, + max_vertical_velocity_m_s, + max_heading_rate_rad_s); + + if (positionReached(target_position_m)) { + _state = State::GoingSouthwest; + } + } + break; + + case State::GoingSouthwest: { + // go to southwest corner while facing the northwestern corner + const Eigen::Vector2d position_of_interest_m = px4_ros2::addVectorToGlobalPosition( + _start_position_m.head( + 2), Eigen::Vector2f{kTriangleHeight, 0.f}); + const float heading_target_rad = px4_ros2::headingToGlobalPosition( + _vehicle_global_position->position().head(2), position_of_interest_m); + + _goto_setpoint->update(_start_position_m, heading_target_rad); + if (positionReached(_start_position_m)) { + completed(px4_ros2::Result::Success); + return; + } + } + break; + } + } + +private: + static constexpr float kTriangleHeight = 20.f; // [m] + static constexpr float kTriangleWidth = 30.f; // [m] + + enum class State + { + SettlingAtStart = 0, + GoingNorth, + GoingEast, + GoingSouthwest + } _state; + + // NED earth-fixed frame. box pattern starting corner (first position the mode sees on activation) + Eigen::Vector3d _start_position_m; + bool _start_position_set{false}; + + // [-pi, pi] current vehicle heading from VehicleAttitude subscription + float _vehicle_heading_rad{0.f}; + + // [-pi, pi] current heading setpoint during spinning phase + float _spinning_heading_rad{0.f}; + + // used for heading initialization when dynamically updating heading setpoints + bool _start_heading_set{false}; + + std::shared_ptr _goto_setpoint; + std::shared_ptr _vehicle_global_position; + rclcpp::Subscription::SharedPtr _vehicle_attitude_sub; + + void updateVehicleHeading(const px4_msgs::msg::VehicleAttitude::UniquePtr & msg) + { + const Eigen::Quaternionf q = Eigen::Quaternionf(msg->q[0], msg->q[1], msg->q[2], msg->q[3]); + const Eigen::Vector3f rpy = px4_ros2::quaternionToEulerRpy(q); + _vehicle_heading_rad = rpy(2); + } + + bool positionReached(const Eigen::Vector3d & target_position_m) const + { + static constexpr float kPositionErrorThreshold = 0.5f; // [m] + const float position_error = px4_ros2::distanceToGlobalPosition( + _vehicle_global_position->position(), target_position_m); + return position_error < kPositionErrorThreshold; + } + + bool headingReached(float target_heading_rad) const + { + static constexpr float kHeadingErrorThreshold = 7.0_deg; + const float heading_error_wrapped = px4_ros2::wrapPi(target_heading_rad - _vehicle_heading_rad); + return fabsf(heading_error_wrapped) < kHeadingErrorThreshold; + } +}; + +class TestNode : public rclcpp::Node +{ +public: + TestNode() + : Node(kNodeName) + { + // Enable debug output + auto ret = + rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str); + rcutils_reset_error(); + } + + _mode = std::make_unique(*this); + + if (!_mode->doRegister()) { + throw std::runtime_error("Registration failed"); + } + } + +private: + std::unique_ptr _mode; +}; diff --git a/examples/cpp/modes/goto_global/package.xml b/examples/cpp/modes/goto_global/package.xml new file mode 100644 index 00000000..8fadf30e --- /dev/null +++ b/examples/cpp/modes/goto_global/package.xml @@ -0,0 +1,26 @@ + + + + example_mode_goto_global_cpp + 0.0.1 + Example mode: Go-to global + Beat Kueng + BSD-3-Clause + + eigen3_cmake_module + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + eigen + + px4_ros2_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/examples/cpp/modes/goto_global/src/main.cpp b/examples/cpp/modes/goto_global/src/main.cpp new file mode 100644 index 00000000..60b60a57 --- /dev/null +++ b/examples/cpp/modes/goto_global/src/main.cpp @@ -0,0 +1,17 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "rclcpp/rclcpp.hpp" + +#include + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}