diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt new file mode 100644 index 0000000..0701bbc --- /dev/null +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -0,0 +1,119 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_formation_control) + +set(CMAKE_CXX_STANDARD 20) + +add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + +find_package(px4_ros2_cpp REQUIRED) + + +add_library(px4_formation_control_lib STATIC + src/formation_control.cpp +) + +target_include_directories(px4_formation_control_lib PUBLIC + $ + $ + ${Eigen3_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} +) + +ament_target_dependencies(px4_formation_control_lib + rclcpp + Eigen3 + px4_ros2_cpp + geometry_msgs + tf2 + tf2_ros +) + +target_link_libraries(px4_formation_control_lib + ${GeographicLib_LIBRARIES} +) + +target_compile_features(px4_formation_control_lib + PUBLIC c_std_99 cxx_std_20) + +add_executable(px4_formation_control + src/main.cpp +) + +add_executable(px4_formation_control_executor_solution + src/solution.cpp +) + +add_executable(px4_formation_control_executor_exercise + src/exercise.cpp +) + +target_link_libraries(px4_formation_control + px4_formation_control_lib +) + +target_link_libraries(px4_formation_control_executor_solution + px4_formation_control_lib +) + +target_link_libraries(px4_formation_control_executor_exercise + px4_formation_control_lib +) + +ament_target_dependencies(px4_formation_control + rclcpp + px4_ros2_cpp +) + +ament_target_dependencies(px4_formation_control_executor_solution + rclcpp + px4_ros2_cpp +) + +ament_target_dependencies(px4_formation_control_executor_exercise + rclcpp + px4_ros2_cpp +) + +target_compile_features(px4_formation_control + PUBLIC c_std_99 cxx_std_20) + +target_compile_features(px4_formation_control_executor_solution + PUBLIC c_std_99 cxx_std_20) + +target_compile_features(px4_formation_control_executor_exercise + PUBLIC c_std_99 cxx_std_20) + +install( + TARGETS + px4_formation_control_lib + px4_formation_control + px4_formation_control_executor_solution + px4_formation_control_executor_exercise + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/README.md b/px4_roscon_workshop/formation_control/README.md new file mode 100644 index 0000000..1fca38e --- /dev/null +++ b/px4_roscon_workshop/formation_control/README.md @@ -0,0 +1,84 @@ +# Formation Control + +The `px4_formation_control` package demonstrates multi-vehicle formation control with PX4 and ROS 2. It launches three simulated X500 vehicles and runs one `Formation` flight mode per vehicle. + +Each controller: + +- publishes the vehicle's local TF frames (`map` and `base_link`), +- reads the relative TF transform to its configured neighbors, and +- commands a horizontal velocity proportional to the error from each desired neighbor distance. + +The default formation is a triangle with 10 m target distances. The vehicles hold an altitude of 2 m above the local origin. + +## Prerequisites + +- A sourced ROS 2 installation with the workspace dependencies available. +- A built PX4-Autopilot SITL target. The launch file uses the executable at `build/px4_sitl_default/bin/px4`. +- Gazebo and the PX4 ROS 2 integration packages installed and built in this workspace. +- QGroundControl, or another way to activate the flight mode and arm the vehicles. + +The PX4 repository path can be supplied explicitly, or through the `PX4_PATH` environment variable. If neither is set, the default is `~/PX4-Autopilot`. + +## Build + +From the workspace root: + +```sh +source /opt/ros/$ROS_DISTRO/setup.bash +colcon build --packages-select px4_formation_control +source install/setup.bash +``` + +## Run + +Launch the simulation and all three formation controllers: + +```sh +ros2 launch px4_formation_control formation.launch.py \ + px4_autopilot_path:=~/PX4-Autopilot +``` + +The `px4_autopilot_path` argument must point to the PX4-Autopilot repository root. The launch file also accepts a Gazebo world name: + +```sh +ros2 launch px4_formation_control formation.launch.py \ + px4_autopilot_path:=~/PX4-Autopilot \ + world:=default +``` + +After startup, use QGroundControl to activate the required mode and arm the vehicles according to the PX4 safety rules. Arming is allowed while the formation mode is active. + +## Executor exercise + +The package includes three executable variants: + +- `px4_formation_control`: the original formation mode. Takeoff then activate `Formation` manually. +- `px4_formation_control_executor_exercise`: the workshop exercise. Complete the node by creating and registering a `px4_ros2::ModeExecutorBase` that takes off and then schedules the formation mode. +- `px4_formation_control_executor_solution`: the completed exercise. Its executor starts with a 2 m takeoff, activates the formation mode when takeoff completes, and waits for disarming when the formation mode finishes. + +The launch file selects the solution by default. To try another variant, change `ACTIVE_EXECUTABLE_IDX` in `launch/formation.launch.py`: + +```python +FORMATION_CONTROL_EXECUTABLE = ( + 'px4_formation_control', + 'px4_formation_control_executor_exercise', + 'px4_formation_control_executor_solution', +) +ACTIVE_EXECUTABLE_IDX = 1 # exercise +``` + +## Default configuration + +| Vehicle | Namespace | Spawn position (m) | Neighbors | +| --- | --- | --- | --- | +| 0 | `/px4_0/` | `(0, 0, 0.3)` | `px4_1`, `px4_2` | +| 1 | `/px4_1/` | `(10, 0, 0.3)` | `px4_0`, `px4_2` | +| 2 | `/px4_2/` | `(5, 8.660254, 0.3)` | `px4_0`, `px4_1` | + +Each controller uses a target distance of `10.0` m and a control gain of `1.0`. These values are configured in `launch/formation.launch.py`. + +## Notes + +- This is an experimental workshop demonstration intended for simulation. +- The controller depends on valid local-position data and TF transforms from the other vehicles. It skips a neighbor while its transform is unavailable. +- Stop the launch process before restarting it to avoid leaving PX4 or Gazebo processes running. diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp new file mode 100644 index 0000000..039a4b9 --- /dev/null +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" + +using namespace px4_ros2::literals; // NOLINT + +class FormationControlMode : public px4_ros2::ModeBase { + public: + explicit FormationControlMode( + rclcpp::Node& node, const std::string& topic_namespace_prefix = ""); + + void onActivate() override; + void updateSetpoint(float dt_s) override; + + private: + GeographicLib::Geocentric _geocentric{GeographicLib::Geocentric::WGS84()}; + std::shared_ptr _vehicle_local_position; + bool _has_global_position{false}; + uint64_t _last_global_ref_timestamp{0}; + std::unique_ptr _tf_broadcaster; + geometry_msgs::msg::TransformStamped _ekf_origin; + const std::string _tf_prefix; + const std::vector _neighbor_distances; + const std::vector _neighbor_prefixes; + const double _gain; + std::vector _neighbor_base_link_frames; + std::shared_ptr _tf_listener{nullptr}; + std::unique_ptr _tf_buffer; + std::shared_ptr _trajectory_setpoint; +}; diff --git a/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp new file mode 100644 index 0000000..2b0cf98 --- /dev/null +++ b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +class FormationExecutor : public px4_ros2::ModeExecutorBase { + public: + explicit FormationExecutor(px4_ros2::ModeBase& formation_mode); + + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; + + private: + enum class State { + Takeoff, + Formation, + WaitUntilDisarmed, + }; + + void switchToState(State state, px4_ros2::Result previous_result); +}; diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py new file mode 100644 index 0000000..8dca4ad --- /dev/null +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -0,0 +1,132 @@ +from os import path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +FORMATION_CONTROL_EXECUTABLE = ( + 'px4_formation_control', + 'px4_formation_control_executor_exercise', + 'px4_formation_control_executor_solution', +) +ACTIVE_EXECUTABLE_IDX = 2 + + +def generate_launch_description() -> LaunchDescription: + px4_roscon_workshop_share = FindPackageShare('px4_roscon_workshop').find( + 'px4_roscon_workshop' + ) + gz_world_launch = path.join( + px4_roscon_workshop_share, + 'launch', + 'gz_world.launch.py', + ) + px4_vehicle_launch = path.join( + px4_roscon_workshop_share, + 'launch', + 'px4_vehicle.launch.py', + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the Gazebo world to launch', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_world_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '0', + 'model': 'x500_0', + 'px4_ns': 'px4_0', + 'spawn_pos_x': '0.0', + 'spawn_pos_y': '0.0', + 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=1,PX4_GZ_NO_FOLLOW=1', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '1', + 'model': 'x500_1', + 'px4_ns': 'px4_1', + 'spawn_pos_x': '10.0', + 'spawn_pos_y': '0.0', + 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '2', + 'model': 'x500_2', + 'px4_ns': 'px4_2', + 'spawn_pos_x': '5.0', + 'spawn_pos_y': '8.660254', + 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', + }.items(), + ), + Node( + package='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name='formation_controller_0', + output='screen', + parameters=[{ + 'px4_ns': '/px4_0/', + 'tf_prefix': 'px4_0', + 'neighbor_prefixes': ['px4_1', 'px4_2'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0, + }], + ), + Node( + package='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name='formation_controller_1', + output='screen', + parameters=[{ + 'px4_ns': '/px4_1/', + 'tf_prefix': 'px4_1', + 'neighbor_prefixes': ['px4_0', 'px4_2'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0, + }], + ), + Node( + package='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name='formation_controller_2', + output='screen', + parameters=[{ + 'px4_ns': '/px4_2/', + 'tf_prefix': 'px4_2', + 'neighbor_prefixes': ['px4_0', 'px4_1'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0, + }], + ), + ]) diff --git a/px4_roscon_workshop/formation_control/package.xml b/px4_roscon_workshop/formation_control/package.xml new file mode 100644 index 0000000..51274fb --- /dev/null +++ b/px4_roscon_workshop/formation_control/package.xml @@ -0,0 +1,32 @@ + + + + px4_formation_control + 0.0.0 + ROSCon workshop Formation Control demo + Beniamino Pozzan + CC-BY-SA-4.0 + + eigen3_cmake_module + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + geographiclib + eigen + geographiclib + + px4_ros2_cpp + geometry_msgs + tf2 + tf2_ros + px4_roscon_workshop + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/src/exercise.cpp b/px4_roscon_workshop/formation_control/src/exercise.cpp new file mode 100644 index 0000000..1189391 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/exercise.cpp @@ -0,0 +1,40 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + /* + The node shall register a mode executor to handle the formation control mode. + The mode executor, once activated, will trigger a takeoff + and then switch to the formation control mode. + */ + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/src/formation_control.cpp b/px4_roscon_workshop/formation_control/src/formation_control.cpp new file mode 100644 index 0000000..171a705 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/formation_control.cpp @@ -0,0 +1,116 @@ +#include + +using namespace px4_ros2::literals; // NOLINT + +static const std::string kName = "Formation"; + +FormationControlMode::FormationControlMode( + rclcpp::Node& node, const std::string& topic_namespace_prefix) + : ModeBase(node, Settings{kName}, topic_namespace_prefix), + _tf_prefix(node.get_parameter("tf_prefix").as_string()), + _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), + _neighbor_prefixes(node.get_parameter("neighbor_prefixes").as_string_array()), + _gain(node.get_parameter("gain").as_double()) +{ + for (const auto& prefix : _neighbor_prefixes) { + _neighbor_base_link_frames.push_back(prefix + "base_link"); + } + _tf_broadcaster = std::make_unique(node); + _tf_buffer = std::make_unique(node.get_clock()); + _tf_listener = std::make_shared(*_tf_buffer); + _trajectory_setpoint = std::make_shared(*this); + _vehicle_local_position = std::make_shared(*this); + _vehicle_local_position->onUpdate([this](const px4_msgs::msg::VehicleLocalPosition& msg) { + if (msg.xy_global && msg.z_global) { + _has_global_position = true; + if (msg.ref_timestamp != _last_global_ref_timestamp) { + _last_global_ref_timestamp = msg.ref_timestamp; + double x, y, z; + _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); + RCLCPP_INFO(this->node().get_logger(), + "Global position reference updated: lat=%f, lon=%f, alt=%f", + msg.ref_lat, msg.ref_lon, msg.ref_alt); + RCLCPP_INFO(this->node().get_logger(), + "ECEF position reference updated: x=%f, y=%f, z=%f", x, y, z); + _ekf_origin.header.frame_id = "earth"; + _ekf_origin.child_frame_id = _tf_prefix + "map"; + _ekf_origin.transform.translation.x = x; + _ekf_origin.transform.translation.y = y; + _ekf_origin.transform.translation.z = z; + double cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); + double sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); + double cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); + double sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); + Eigen::Matrix3d R; + R(0, 0) = -sin_lon; + R(1, 0) = cos_lon; + R(2, 0) = 0.0; + R(0, 1) = -sin_lat * cos_lon; + R(1, 1) = -sin_lat * sin_lon; + R(2, 1) = cos_lat; + R(0, 2) = cos_lat * cos_lon; + R(1, 2) = cos_lat * sin_lon; + R(2, 2) = sin_lat; + Eigen::Quaterniond q(R); + _ekf_origin.transform.rotation.x = q.x(); + _ekf_origin.transform.rotation.y = q.y(); + _ekf_origin.transform.rotation.z = q.z(); + _ekf_origin.transform.rotation.w = q.w(); + } + _ekf_origin.header.stamp = this->node().get_clock()->now(); + _tf_broadcaster->sendTransform(_ekf_origin); + } else { + _has_global_position = false; + } + if (msg.xy_valid && msg.z_valid) { + const Eigen::Vector3f pos_ned = _vehicle_local_position->positionNed(); + const Eigen::Vector3f pos_enu = px4_ros2::positionNedToEnu(pos_ned); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = this->node().get_clock()->now(); + tf_msg.header.frame_id = _tf_prefix + "map"; + tf_msg.child_frame_id = _tf_prefix + "base_link"; + tf_msg.transform.translation.x = pos_enu(0); + tf_msg.transform.translation.y = pos_enu(1); + tf_msg.transform.translation.z = pos_enu(2); + tf_msg.transform.rotation.x = 0.0; + tf_msg.transform.rotation.y = 0.0; + tf_msg.transform.rotation.z = 0.0; + tf_msg.transform.rotation.w = 1.0; + _tf_broadcaster->sendTransform(tf_msg); + } + }); + RCLCPP_INFO(this->node().get_logger(), + "FormationControlMode initialized with %zu neighbors, gain=%f", + _neighbor_base_link_frames.size(), _gain); +} + +void FormationControlMode::onActivate() +{ +} + +void FormationControlMode::updateSetpoint(float dt_s) +{ + Eigen::Vector2f velocity_en{0.0, 0.0}; + for (const auto& toFrameRel : _neighbor_base_link_frames) { + geometry_msgs::msg::TransformStamped t; + try { + t = _tf_buffer->lookupTransform( + toFrameRel, _tf_prefix + "base_link", tf2::TimePointZero); + const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; + const float distance = relative_en.norm(); + const Eigen::Vector2f direction = relative_en.normalized(); + const float distance_error = + distance - _neighbor_distances[&toFrameRel - &_neighbor_base_link_frames[0]]; + const Eigen::Vector2f individual_control = -distance_error * direction * _gain; + velocity_en.x() += individual_control.x(); + velocity_en.y() += individual_control.y(); + } catch (const tf2::TransformException& ex) { + (void)ex; + } + } + const px4_ros2::TrajectorySetpoint setpoint = px4_ros2::TrajectorySetpoint() + .withVelocityX(velocity_en.y()) + .withVelocityY(velocity_en.x()) + .withPositionZ(-2.0f); + _trajectory_setpoint->update(setpoint); +} diff --git a/px4_roscon_workshop/formation_control/src/main.cpp b/px4_roscon_workshop/formation_control/src/main.cpp new file mode 100644 index 0000000..71cc396 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/main.cpp @@ -0,0 +1,40 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + FormationControlMode mode(*node, node->get_parameter("px4_ns").as_string()); + if (!mode.doRegister()) { + RCLCPP_ERROR(node->get_logger(), "Formation mode registration failed"); + rclcpp::shutdown(); + return 1; + } + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/src/solution.cpp b/px4_roscon_workshop/formation_control/src/solution.cpp new file mode 100644 index 0000000..2bbab30 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/solution.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +FormationExecutor::FormationExecutor(px4_ros2::ModeBase& formation_mode) + : ModeExecutorBase(Settings{}, formation_mode) +{ +} + +void FormationExecutor::onActivate() +{ + switchToState(State::Takeoff, px4_ros2::Result::Success); +} + +void FormationExecutor::onDeactivate(DeactivateReason reason) +{ + RCLCPP_INFO(node().get_logger(), "Formation executor deactivated: %d", + static_cast(reason)); +} + +void FormationExecutor::switchToState(State state, px4_ros2::Result previous_result) +{ + if (previous_result != px4_ros2::Result::Success) { + RCLCPP_WARN(node().get_logger(), "Formation sequence stopped in state %d with result %d", + static_cast(state), static_cast(previous_result)); + return; + } + + switch (state) { + case State::Takeoff: + takeoff( + [this](px4_ros2::Result result) { + switchToState(State::Formation, result); + }, + 2.0f); + break; + case State::Formation: + scheduleMode(ownedMode().id(), + [this](px4_ros2::Result result) { + switchToState(State::WaitUntilDisarmed, result); + }); + break; + case State::WaitUntilDisarmed: + waitUntilDisarmed([](px4_ros2::Result) {}); + break; + } +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + FormationControlMode mode(*node, node->get_parameter("px4_ns").as_string()); + FormationExecutor executor(mode); + if (!executor.doRegister()) { + RCLCPP_ERROR(node->get_logger(), "Formation executor registration failed"); + rclcpp::shutdown(); + return 1; + } + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py new file mode 100644 index 0000000..778c2dd --- /dev/null +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py @@ -0,0 +1,103 @@ +from os import environ, path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + SetEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def _launch_setup(context): + px4_autopilot_path = path.expanduser( + LaunchConfiguration('px4_autopilot_path').perform(context) + ) + world_name = LaunchConfiguration('world').perform(context) + extra_gz_resource_path = LaunchConfiguration('extra_gz_resource_path').perform(context) + bridge_config_file = LaunchConfiguration('bridge_config_file').perform(context) + + ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') + px4_gz_resource_path = path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') + px4_gz_plugin_path = path.join( + px4_autopilot_path, + 'build', + 'px4_sitl_default', + 'src', + 'modules', + 'simulation', + 'gz_plugins', + ) + px4_gz_server_config_path = path.join( + px4_autopilot_path, + 'src', + 'modules', + 'simulation', + 'gz_bridge', + 'server.config', + ) + gz_launch_path = path.join(ros_gz_sim_pkg_path, 'launch', 'ros_gz_sim.launch.py') + + return [ + SetEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + ':'.join([ + path.join(px4_gz_resource_path, 'worlds'), + path.join(px4_gz_resource_path, 'models'), + extra_gz_resource_path, + ]), + ), + SetEnvironmentVariable( + 'GZ_SIM_SYSTEM_PLUGIN_PATH', + ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), px4_gz_plugin_path]), + ), + SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', px4_gz_server_config_path), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_launch_path), + launch_arguments={ + 'world_sdf_file': f'{world_name}.sdf', + 'bridge_name': 'gz_ros_bridge', + 'config_file': bridge_config_file, + }.items(), + ), + ExecuteProcess( + cmd=['MicroXRCEAgent', 'udp4', '--port', '8888', '-v', '1'], + name='microxrce_agent', + output='screen', + ), + ] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the world to launch', + ), + DeclareLaunchArgument( + 'extra_gz_resource_path', + default_value='', + description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', + ), + DeclareLaunchArgument( + 'bridge_config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('px4_roscon_workshop'), + 'cfg', + 'clock_bridge.yaml', + ]), + description='Path to the ROS-GZ bridge configuration file', + ), + OpaqueFunction(function=_launch_setup), + ]) \ No newline at end of file diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py index bb72dff..f41fbb3 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py @@ -1,94 +1,17 @@ -import glob -from os import path, environ -from ament_index_python.packages import get_package_share_directory +from os import path from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription, ExecuteProcess, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -# launch gazebo server and PX4 -# gz gui needs to be launched separately, e.g.: gz sim -g - -def _launch_setup(context): - px4_autopilot_path = path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) - ) - world_name = LaunchConfiguration('world').perform(context) - model_name = LaunchConfiguration('model').perform(context) - model_path = path.expanduser( - LaunchConfiguration('model_path').perform(context) - ) - - extra_gz_resource_path = LaunchConfiguration('extra_gz_resource_path').perform(context) - - ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') - px4_roscon_workshop_pkg_path = get_package_share_directory('px4_roscon_workshop') - px4_gz_resource_path = path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') - px4_gz_plugin_path = path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'src', 'modules', 'simulation', 'gz_plugins') - px4_gz_server_config_path = path.join(px4_autopilot_path, 'src', 'modules', 'simulation', 'gz_bridge', 'server.config') - gz_launch_path = path.join(ros_gz_sim_pkg_path, 'launch', 'ros_gz_sim.launch.py') - gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') - - return [ - SetEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - ':'.join([path.join(px4_gz_resource_path, 'worlds'), path.join(px4_gz_resource_path, 'models'), extra_gz_resource_path]) - ), - SetEnvironmentVariable( - 'GZ_SIM_SYSTEM_PLUGIN_PATH', - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), px4_gz_plugin_path]) - ), - SetEnvironmentVariable( - 'GZ_SIM_SERVER_CONFIG_PATH', - px4_gz_server_config_path - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(gz_launch_path), - launch_arguments={ - 'world_sdf_file': f'{world_name}.sdf', - 'bridge_name': 'gz_ros_bridge', - 'config_file': path.join(px4_roscon_workshop_pkg_path, 'cfg', 'clock_bridge.yaml') - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(gz_spawn_path), - launch_arguments={ - 'world': world_name, - 'file': model_path, - }.items(), - ), - ExecuteProcess( - cmd = [ - path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), - ], - additional_env = { - 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), - 'PX4_GZ_MODEL_NAME': model_name, - 'PX4_GZ_STANDALONE': '1', - 'PX4_GZ_WORLD': world_name, - }, - name = 'px4', - output = 'screen' - ), - ExecuteProcess( - cmd = [ - "MicroXRCEAgent", - "udp4", - "--port", "8888", - ], - name = 'microxrce_agent', - output = 'screen' - ) - ] - - def generate_launch_description(): + pkg_share = FindPackageShare('px4_roscon_workshop').find('px4_roscon_workshop') + return LaunchDescription([ DeclareLaunchArgument( 'px4_autopilot_path', - default_value='~/PX4-Autopilot', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), description='Path to PX4-Autopilot repository root (supports ~)', ), DeclareLaunchArgument( @@ -103,7 +26,15 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'model_path', - default_value='~/PX4-Autopilot/Tools/simulation/gz/models/x500/model.sdf', + default_value=PathJoinSubstitution([ + LaunchConfiguration('px4_autopilot_path'), + 'Tools', + 'simulation', + 'gz', + 'models', + 'x500', + 'model.sdf', + ]), description='Path to the model SDF file', ), DeclareLaunchArgument( @@ -116,5 +47,32 @@ def generate_launch_description(): default_value='', description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', ), - OpaqueFunction(function=_launch_setup), + DeclareLaunchArgument( + 'bridge_config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('px4_roscon_workshop'), + 'cfg', + 'clock_bridge.yaml', + ]), + description='Path to the ROS-GZ bridge configuration file', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'gz_world.launch.py')), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'extra_gz_resource_path': LaunchConfiguration('extra_gz_resource_path'), + 'bridge_config_file': LaunchConfiguration('bridge_config_file'), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'px4_vehicle.launch.py')), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'model': LaunchConfiguration('model'), + 'model_path': LaunchConfiguration('model_path'), + 'px4_autostart_id': LaunchConfiguration('px4_autostart_id'), + }.items(), + ), ]) \ No newline at end of file diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py new file mode 100644 index 0000000..73e1440 --- /dev/null +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py @@ -0,0 +1,134 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution + + +def _launch_setup(context): + px4_autopilot_path = path.expanduser( + LaunchConfiguration('px4_autopilot_path').perform(context) + ) + world_name = LaunchConfiguration('world').perform(context) + model_name = LaunchConfiguration('model').perform(context) + model_path = path.expanduser( + LaunchConfiguration('model_path').perform(context) + ) + px4_instance = LaunchConfiguration('px4_instance').perform(context) + px4_namespace = LaunchConfiguration('px4_ns').perform(context) + + px4_extra_env_vars = LaunchConfiguration('px4_extra_env_vars').perform(context) + + ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') + gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') + + px4_env = { + 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), + 'PX4_GZ_MODEL_NAME': model_name, + 'PX4_GZ_STANDALONE': '1', + 'PX4_GZ_WORLD': world_name, + } + px4_env.update( + dict( + (key.strip(), value.strip()) + for env_var in px4_extra_env_vars.split(',') + if env_var.strip() + for key, value in [env_var.split('=', 1)] + ) + ) + if px4_namespace: + px4_env['PX4_UXRCE_DDS_NS'] = px4_namespace + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_spawn_path), + launch_arguments={ + 'world': world_name, + 'file': model_path, + 'entity_name': model_name, + 'x': LaunchConfiguration('spawn_pos_x').perform(context), + 'y': LaunchConfiguration('spawn_pos_y').perform(context), + 'z': LaunchConfiguration('spawn_pos_z').perform(context), + }.items(), + ), + ExecuteProcess( + cmd=[ + path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), + '-i', px4_instance, + ], + additional_env=px4_env, + name=f'px4_{px4_instance}', + output='screen', + ), + ] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'px4_instance', + default_value='0', + description='PX4 instance ID', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the world where the model is spawned', + ), + DeclareLaunchArgument( + 'model', + default_value='x500', + description='Name of the model to start PX4 for', + ), + DeclareLaunchArgument( + 'model_path', + default_value=PathJoinSubstitution([ + LaunchConfiguration('px4_autopilot_path'), + 'Tools', + 'simulation', + 'gz', + 'models', + 'x500', + 'model.sdf', + ]), + description='Path to the model SDF file', + ), + DeclareLaunchArgument( + 'px4_autostart_id', + default_value='4001', + description='PX4 autostart ID', + ), + DeclareLaunchArgument( + 'px4_ns', + default_value='', + description='PX4 namespace', + ), + DeclareLaunchArgument( + 'px4_extra_env_vars', + default_value='', + description='Extra environment variables to set for PX4', + ), + DeclareLaunchArgument( + 'spawn_pos_x', + default_value='0.0', + description='X position to spawn the model at', + ), + DeclareLaunchArgument( + 'spawn_pos_y', + default_value='0.0', + description='Y position to spawn the model at', + ), + DeclareLaunchArgument( + 'spawn_pos_z', + default_value='0.0', + description='Z position to spawn the model at', + ), + OpaqueFunction(function=_launch_setup), + ]) \ No newline at end of file diff --git a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py index 5ca0bf8..521259d 100644 --- a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py +++ b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py @@ -7,7 +7,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, EnvironmentVariable @@ -19,7 +19,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( 'px4_autopilot_path', - default_value='~/PX4-Autopilot', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), description='Path to PX4-Autopilot repository root (supports ~)', ), IncludeLaunchDescription(