Gazebo: Moving platform (#24471)

* gz_plugins: add MovingPlatformController This plugin moves the entity to which it is attached to simulate moving platforms (boats, trucks, ...) to takeoff and land on. Updates Tools/simulation/gz submodule with corresponding dependency. Use with: PX4_GZ_MODEL_POSE=0,0,2.2,0,0,0 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol more in README.md * MovingPlatformController: Wrench implementation Now it works by applying appropriate forces & torques to make the platform move as desired. Compared to the previous velocity-based version it introduces no kinematic constraints, keeping it realistic. Other updates: - Also make heading configurable by env var in addition to velocity - Cleaner error handling (runtime error, gzerr, gzwarn) - Read parameters (gravity, platform mass & height) from model rather than hardcoding - Update README with new env vars, usage in sdf, etc. * MovingPlatformController: fix warning message * MovingPlatformController: fix build https://github.com/PX4/PX4-Autopilot/pull/24518 changed some variable names in CMakeLists. This adapts ours to use the new ones. * MovingPlatformController: format * MovingPlatformController: address code review From feedback on PR - Parameterise low pass filters with cutoff frequency (rather than filter coefficient directly). - Add comment with units of feedback gains. - Scale attitude gains with platform inertia (rather than mass). Additionally - Wait 5 seconds before moving the platform so the model has time to spawn (was quicker before rebasing...) - Refactor: separate noise generation and force/torque calculation into two separate functions - rename updatePlatformState -> getPlatformState to emphasise difference from other update* functions that update internal state only - remove unused gz transport node - README grammar * MovingPlatformController: format * MovingPlatformController: remove redundant call * MovingPlatformController: clarify explanation * MovingPlatformController: clarify & comment units * MovingPlatformController: wait for model to spawn Rather than waiting a fixed 5s, we now only move the platform once the model is spawned. For that we construct the model name from the relevant environment variables, in the same way as done in px4-rc.gzsim. If attaching to an existing model, do not wait. * MovingPlatformController: correct substring extraction --------- Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
2026-04-14 10:07:39 +08:00 · 2025-04-06 20:57:44 +02:00 · 2025-04-06 20:57:44 +02:00 · c0bb482126
commit c0bb482126
parent 561937f35a
6 changed files with 632 additions and 3 deletions
--- a/Tools/simulation/gz
+++ b/Tools/simulation/gz
@ -1 +1 @@
-Subproject commit 6caa6d54fa95b0141c9974bbd836b0f6fe0f26ee
+Subproject commit e05f4312d3f28aa621157610584a4870406cb6d3
--- a/src/modules/simulation/gz_plugins/CMakeLists.txt
+++ b/src/modules/simulation/gz_plugins/CMakeLists.txt
@ -55,11 +55,12 @@ if (gz-transport_FOUND)
    add_subdirectory(optical_flow)
    add_subdirectory(template_plugin)
    add_subdirectory(gstreamer)
+    add_subdirectory(moving_platform_controller)

    # Add an alias target for each plugin
    if (TARGET GstCameraSystem)
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin GstCameraSystem)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GstCameraSystem)
    else()
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin)
    endif()
 endif()
--- a/src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt
@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2025 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 PX4 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 OWNER 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.
+#
+############################################################################
+
+project(MovingPlatformController)
+
+add_library(${PROJECT_NAME} SHARED
+    MovingPlatformController.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC px4_gz_msgs
+    PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
+    PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
+    PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
+    PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+)
--- a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
@ -0,0 +1,395 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 PX4 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 OWNER 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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_world_entity = gz::sim::worldEntity(ecm);
+	_world = gz::sim::World(_world_entity);
+
+	getVehicleModelName();
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = readEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = readEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto gravity = _world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Keep stationary if model is not yet spawned (and model name valid).
+	// If model name invalid, then we don't know what to wait for and move it immediately.
+	const bool vehicle_has_spawned = 0 != _world.ModelByName(ecm, _vehicle_model_name);
+	const bool keep_stationary = _wait_for_vehicle_spawned && !vehicle_has_spawned;
+
+	updateWrenchCommand(_velocity_sp, _orientation_sp, keep_stationary);
+
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateNoise(const double dt)
+{
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					 );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Cutoff frequencies for low-pass filtering the white noise.
+	// larger number = faster movement
+	// Should be between 0 and sample_freq / 2, with sample_freq = 1/dt
+	const double cutoff_freq_force  = 5.;  // 1/s
+	const double cutoff_freq_torque = 5.;  // 1/s
+
+	const double max_cutoff_freq = 0.5 / dt;
+
+	if (cutoff_freq_force < 0 || cutoff_freq_force > max_cutoff_freq ||
+	    cutoff_freq_torque < 0 || cutoff_freq_torque > max_cutoff_freq) {
+		throw std::runtime_error("MovingPlatformController::updateWrenchCommand: invalid cutoff frequency");
+	}
+
+	// Cutoff frequency -> filter coefficient calculation from PX4's AlphaFilter.hpp
+	// from AlphaFilter::setCutoffFreq
+	const double time_constant_force = 1. / (2 * GZ_PI * cutoff_freq_force);
+	const double time_constant_torque = 1. / (2 * GZ_PI * cutoff_freq_torque);
+
+	// from AlphaFilter::setParameters
+	const double filter_coef_force = dt / (dt + time_constant_force);
+	const double filter_coef_torque = dt / (dt + time_constant_torque);
+
+	// This noise is dimensionless, units only come into play in updateWrenchCommand
+	_noise_lowpass_force = (1. - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
+	_noise_lowpass_torque = (1. - filter_coef_torque) * _noise_lowpass_torque + filter_coef_torque * noise_torque;
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint,
+	const bool keep_stationary)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	// If keep_stationary, override noise amplitude and velocity setpoint to zero.
+	// This is to wait for the model to spawn initially.
+
+	// Noise amplitude >= 0
+	// larger number = bigger movement
+	const double noise_ampl_force_scaling = keep_stationary ? 0. : 1.;          // [N / kg]
+	const double noise_ampl_force = noise_ampl_force_scaling * _platform_mass;  // [N]
+
+	const double noise_ampl_torque_scaling = keep_stationary ? 0. : 1.;         // [N m / (kg m^2) = 1/s^2]
+	const gz::math::Vector3d noise_ampl_torque = noise_ampl_torque_scaling * _platform_diag_moments; // [N m]
+
+	const gz::math::Vector3d normal_force(0., 0., -_gravity * _platform_mass);  // [N]
+
+	_force = noise_ampl_force * _noise_lowpass_force + normal_force;            // [N]
+	_torque = noise_ampl_torque * _noise_lowpass_torque;                        // [N m]
+
+	// Feedback terms to ensure stability of the platform.
+	{
+
+		// Position - simple PD controller, but with zero position gains in xy direction.
+		// With the Vector3d on the RHS having units of 1/s^2 and 1/s, respectively
+		const gz::math::Vector3d pos_gains = _platform_mass * gz::math::Vector3d(0., 0., 1.); // [N / m]
+		const gz::math::Vector3d vel_gains = _platform_mass * gz::math::Vector3d(1., 1., 1.); // [N / (m/s)]
+
+		const gz::math::Vector3d platform_position_setpoint(0., 0., _platform_height_setpoint);
+		const gz::math::Vector3d current_velocity_setpoint = keep_stationary ? gz::math::Vector3d::Zero : velocity_setpoint;
+
+		const gz::math::Vector3d platform_pos_error = (_platform_position - platform_position_setpoint);
+		const gz::math::Vector3d platform_vel_error = (_platform_velocity - current_velocity_setpoint);
+
+		// * is element-wise
+		const gz::math::Vector3d feedback_force = -pos_gains * platform_pos_error - vel_gains * platform_vel_error;
+
+		// Clip horizontal force to avoid large accelerations, which might cause the drone to slip off the platform.
+		const float max_accel = 2.; // [m/s^2]
+		const gz::math::Vector2d _force_xy = gz::math::Vector2d(feedback_force.X(), feedback_force.Y());
+		const float accel_xy = _force_xy.Length() / _platform_mass;
+
+		if (accel_xy > max_accel) {
+			const float scaling = max_accel / accel_xy;
+			_force += feedback_force * gz::math::Vector3d(scaling, scaling, 1.);
+
+		} else {
+			_force += feedback_force;
+		}
+
+		// Attitude - similar but accounting for quaternion weirdness.
+		// Combining ideas from:
+		//  - Eq. 23 in Nonlinear Quadrocopter Attitude Control (Brescianini, Hehn, D'Andrea)
+		//    https://www.research-collection.ethz.ch/handle/20.500.11850/154099
+		//  - Eq. 20 in Full Quaternion Based Attitude Control for a Quadrotor (Fresk, Nikolakopoulos)
+		//    https://www.diva-portal.org/smash/get/diva2:1010947/FULLTEXT01.pdf
+
+		const gz::math::Quaterniond attitude_err = _platform_orientation * orientation_setpoint.Inverse();
+
+		// With the factors of 1. having units of 1 / (m rad) and s / (m rad), respectively
+		const gz::math::Vector3d attitude_p_gain = 1. * _platform_diag_moments; // [N m / rad]
+		const gz::math::Vector3d attitude_d_gain = 1. * _platform_diag_moments; // [N m / (rad/s)]
+
+		const double sgn = attitude_err.W() > 0. ? 1. : -1.;
+		gz::math::Vector3d attitude_err_imag = sgn * gz::math::Vector3d(attitude_err.X(), attitude_err.Y(), attitude_err.Z());
+
+		// Factor of 2 to convert quaternion error to rad
+		_torque += -2. * attitude_p_gain * attitude_err_imag - attitude_d_gain * _platform_angular_velocity;
+	}
+}
+
+
+void MovingPlatformController::getPlatformState(const gz::sim::EntityComponentManager &ecm)
+{
+
+	const auto optional_pose = _link.WorldPose(ecm);
+
+	if (optional_pose.has_value()) {
+		_platform_position = optional_pose.value().Pos();
+		_platform_orientation = optional_pose.value().Rot();
+
+	} else {
+		gzerr << "Unable to get pose" << std::endl;
+	}
+
+
+	const auto optional_vel = _link.WorldLinearVelocity(ecm);
+
+	if (optional_vel.has_value()) {
+		_platform_velocity = optional_vel.value();
+
+	} else {
+		gzerr << "Unable to get linear velocity" << std::endl;
+	}
+
+
+	const auto optional_angular_vel = _link.WorldAngularVelocity(ecm);
+
+	if (optional_angular_vel.has_value()) {
+		_platform_angular_velocity = optional_angular_vel.value();
+
+	} else {
+		gzerr << "Unable to get angular velocity" << std::endl;
+	}
+
+}
+
+void MovingPlatformController::sendWrenchCommand(gz::sim::EntityComponentManager &ecm)
+{
+	_link.AddWorldWrench(ecm, _force, _torque);
+}
+
+
+double MovingPlatformController::readEnvVar(const char *env_var_name, double default_value)
+{
+
+	const char *env_var_value = std::getenv(env_var_name);
+
+	if (env_var_value) {
+		try {
+			double value = std::stod(env_var_value);
+			return value;
+
+		} catch (const std::invalid_argument &e) {
+			// These warnings will only be visible with sufficient verbosity level...
+			gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+			gzwarn << "Keeping default value of " << default_value << " m/s." << std::endl;
+
+		} catch (const std::out_of_range &e) {
+			gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+			gzwarn << "Keeping default value of " << default_value << " m/s." << std::endl;
+		}
+	}
+
+	return default_value;
+}
+
+void MovingPlatformController::getVehicleModelName()
+{
+	// Find the name of the gazebo vehicle model.
+	// The name is constructed in px4-rc.gzsim as:
+	//     MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
+	//     MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
+	// So here we replicate that.
+
+	const char *px4_sim_model_cstr = std::getenv("PX4_SIM_MODEL");
+	std::string px4_sim_model = "";
+
+	const char *px4_gz_model_name_cstr = std::getenv("PX4_GZ_MODEL_NAME");
+
+	if (px4_sim_model_cstr != nullptr) {
+
+		px4_sim_model = px4_sim_model_cstr;
+
+	} else if (px4_gz_model_name_cstr != nullptr) {
+
+		// This happens if we attach to an existing model. In this case,
+		// do not wait for any vehicle to spawn.
+
+		gzwarn << "PX4_SIM_MODEL not set. Proceeding without vehicle name and moving platform immediately." << std::endl;
+		_wait_for_vehicle_spawned = false;
+
+	} else {
+
+		// If neither are set, the px4-rc.gzsim script should have
+		// exited 1. We could land here if these environment variables
+		// change -- if so, update this function accordingly.
+
+		gzerr << "Neither PX4_MODEL nor PX4_GZ_MODEL_NAME are set. One needed to proceed." << std::endl;
+		_wait_for_vehicle_spawned = false;
+	}
+
+	// Remove leading "gz_"
+	const std::string prefix_to_remove = "gz_";
+	size_t pos = px4_sim_model.find(prefix_to_remove);
+
+	if (pos != std::string::npos) {
+		px4_sim_model = px4_sim_model.substr(pos + prefix_to_remove.length());
+
+	} else {
+		gzwarn << "Error: \"gz_\" not found in PX4_SIM_MODEL. Using the entire string as MODEL_NAME." << std::endl;
+	}
+
+	// Get the px4_instance environment variable
+	const char *px4_instance_cstr = std::getenv("px4_instance");
+	std::string px4_instance = "";
+
+	if (px4_instance_cstr != nullptr) {
+		px4_instance = px4_instance_cstr;
+
+	} else {
+		px4_instance = "0";
+	}
+
+	_vehicle_model_name = px4_sim_model + "_" + px4_instance;
+	_wait_for_vehicle_spawned = true;
+}
--- a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 PX4 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 OWNER 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <gz/sim/Util.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/World.hh>
+#include <gz/sim/System.hh>
+#include "gz/sim/components/Pose.hh"
+#include <gz/sim/components/Model.hh>
+#include "gz/sim/components/Inertial.hh"
+#include "gz/sim/components/LinearVelocity.hh"
+#include "gz/sim/components/AngularVelocity.hh"
+#include <gz/sim/EntityComponentManager.hh>
+
+#include <gz/common/Timer.hh>
+
+#include <gz/plugin/Register.hh>
+
+#include <gz/msgs/Utility.hh>
+#include <gz/msgs/twist.pb.h>
+
+#include <gz/math.hh>
+#include <gz/math/Rand.hh>
+#include <gz/math/Pose3.hh>
+
+using namespace std::chrono_literals;
+
+namespace custom
+{
+class MovingPlatformController:
+	public gz::sim::System,
+	public gz::sim::ISystemPreUpdate,
+	public gz::sim::ISystemConfigure
+{
+public:
+	void PreUpdate(const gz::sim::UpdateInfo &_info,
+		       gz::sim::EntityComponentManager &_ecm) final;
+
+	void Configure(const gz::sim::Entity &entity,
+		       const std::shared_ptr<const sdf::Element> &sdf,
+		       gz::sim::EntityComponentManager &ecm,
+		       gz::sim::EventManager &eventMgr) override;
+
+private:
+
+	void getPlatformState(const gz::sim::EntityComponentManager &ecm);
+	void updateNoise(const double dt);
+	void updateWrenchCommand(const gz::math::Vector3d &velocity_setpoint,
+				 const gz::math::Quaterniond &orientation_setpoint,
+				 const bool keep_stationary);
+	void sendWrenchCommand(gz::sim::EntityComponentManager &ecm);
+	double readEnvVar(const char *env_var_name, double default_value);
+	void getVehicleModelName();
+
+	gz::sim::Entity _entity;
+	gz::sim::Model _model{gz::sim::kNullEntity};
+	gz::sim::Entity _link_entity;
+	gz::sim::Link _link;
+
+	gz::sim::Entity _world_entity;
+	gz::sim::World _world;
+
+	// Low-pass filtered white noise for driving boat motion.
+	gz::math::Vector3d _noise_lowpass_force{0., 0., 0.};
+	gz::math::Vector3d _noise_lowpass_torque{0., 0., 0.};
+
+	// Platform linear & angular velocity.
+	gz::math::Vector3d _force{0., 0., 0.};
+	gz::math::Vector3d _torque{0., 0., 0.};
+
+	// Platform position & orientation for feedback.
+	gz::math::Vector3d _platform_position{0., 0., 0.};
+	gz::math::Quaterniond _platform_orientation{1., 0., 0., 0.};
+	gz::math::Vector3d _platform_velocity{0., 0., 0.};
+	gz::math::Vector3d _platform_angular_velocity{0., 0., 0.};
+
+	// Platform velocity setpoint [m/s].
+	gz::math::Vector3d _velocity_sp{1., 0., 0.};
+	// Orientation setpoint.
+	gz::math::Quaterniond _orientation_sp{1., 0., 0., 0.};
+	// Height setpoint [m]
+	double _platform_height_setpoint{2.};
+
+	double _gravity{-9.8};
+	double _platform_mass{10000.};
+	gz::math::Vector3d _platform_diag_moments;
+
+	gz::common::Timer _startup_timer;
+	std::string _vehicle_model_name;
+	bool _wait_for_vehicle_spawned;
+};
+} // end namespace custom
--- a/src/modules/simulation/gz_plugins/moving_platform_controller/README.md
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/README.md
@ -0,0 +1,56 @@
+# Moving Platform Controller
+
+This plugin controls a moving platform that emulates ships/trucks/etc. to
+takeoff and land on. The platform moves at a constant mean velocity, with added
+random fluctuations in velocity and angular velocity.
+
+## Dependencies
+
+This depends on the [moving platform world](https://github.com/PX4/PX4-gazebo-models/blob/moving_platform_world/worlds/moving_platform.sdf) in the `PX4-gazebo-models` repo, so ensure that the `Tools/simulation/gz` submodule is recent enough. That world contains the corresponding [moving platform model](https://github.com/PX4/PX4-gazebo-models/blob/moving_platform_world/models/moving_platform/model.sdf). Within that, we include this plugin.
+
+
+## Usage & Configuration
+
+Start by selecting the moving_platform world, which loads the plugin. We need to set the pose so the aircraft is above the platform, which is at a height of 2m.
+
+```
+PX4_GZ_MODEL_POSE=0,0,2.2 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
+```
+
+The velocity (in m/s) can be set with the `PX4_GZ_PLATFORM_VEL` and `PX4_GZ_PLATFORM_HEADING_DEG` environment variables. By default it is 1 m/s in east direction. The heading is such that 0 represents east, 90 north, 180 west, and 270 south.
+
+```
+PX4_GZ_PLATFORM_VEL=5 PX4_GZ_PLATFORM_HEADING_DEG=135 PX4_GZ_MODEL_POSE=0,0,2.2,0,0,0 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
+```
+
+To use the plugin with a *different* world or model, add the following to the model.sdf:
+
+```
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.9">
+  <model name="flat_platform">
+
+    <!-- the rest of your model -->
+
+    <link name="platform_link">
+      <!-- define the link representing the platform -->
+    </link>
+
+    <plugin
+      filename="libMovingPlatformController.so"
+      name="custom::MovingPlatformController">
+      <link_name>platform_link</link_name>
+    </plugin>
+
+  </model>
+</sdf>
+```
+
+## Limitations & Future Ideas
+
+ - Apart from the velocity and heading, nothing is configurable: Noise amplitude and frequency spectrum, initial acceleration, feedback gains.
+    - Feel free to change these ad-hoc in code, or open an issue / propose a PR for better config options.
+ - This plugin does not communicate the state of the platform with PX4. If that is needed, there are a couple options:
+    - Read the pose of the platform in `GZBridge::poseInfoCallback`
+    - Add an IMU sensor to the platform link, listen to it and the existing NavSat sensor from `GZBridge`.
+    - Add a custom gazebo transport message containing all needed info about the platform, populate and publish it from the plugin here, listen to that in `GZBridge`.