feat: add formation control package

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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${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()

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 (`<prefix>map` and `<prefix>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.

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 <Eigen/Core>
+#include <algorithm>
+#include <GeographicLib/Geocentric.hpp>
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/odometry/local_position.hpp>
+#include <px4_ros2/utils/geometry.hpp>
+#include <px4_ros2/utils/frame_conversion.hpp>
+#include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
+#include <rclcpp/rclcpp.hpp>
+#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<px4_ros2::OdometryLocalPosition> _vehicle_local_position;
+  bool _has_global_position{false};
+  uint64_t _last_global_ref_timestamp{0};
+	std::unique_ptr<tf2_ros::TransformBroadcaster> _tf_broadcaster;
+	geometry_msgs::msg::TransformStamped _ekf_origin;
+	const std::string _tf_prefix;
+	const std::vector<double> _neighbor_distances;
+	const std::vector<std::string> _neighbor_prefixes;
+	const double _gain;
+	std::vector<std::string> _neighbor_base_link_frames;
+	std::shared_ptr<tf2_ros::TransformListener> _tf_listener{nullptr};
+  std::unique_ptr<tf2_ros::Buffer> _tf_buffer;
+  std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+};

px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <px4_ros2/components/mode_executor.hpp>
+
+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);
+};