Add Gazebo MotorFailure Plugin (#25776)

* Add Gazebo MotorFailure Plugin

* switch from ROS2 to Gazebo Transport

* Clean up old/dead comments, Refactor variable

* gz: submodule update

src/modules/simulation/gz_plugins/CMakeLists.txt

@@ -70,11 +70,12 @@ if (gz-transport_FOUND)
     add_subdirectory(generic_motor)
     add_subdirectory(buoyancy)
     add_subdirectory(spacecraft_thruster)
+    add_subdirectory(motor_failure)
 
     # Add an alias target for each plugin
     if (TARGET GstCameraSystem)
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin)
     else()
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin)
     endif()
 endif()

src/modules/simulation/gz_plugins/motor_failure/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(MotorFailurePlugin)
+
+add_library(${PROJECT_NAME} SHARED
+    MotorFailureSystem.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC px4_gz_msgs
+    PUBLIC ${GZ_SENSORS_TARGET}
+    PUBLIC ${GZ_PLUGIN_TARGET}
+    PUBLIC ${GZ_SIM_TARGET}
+    PUBLIC ${GZ_TRANSPORT_TARGET}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+)

src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.cpp

@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   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 "MotorFailureSystem.hpp"
+
+#include <gz/plugin/Register.hh>
+#include <gz/sim/components/JointVelocityCmd.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/Util.hh>
+
+using namespace gz;
+using namespace sim;
+using namespace systems;
+
+//////////////////////////////////////////////////
+void MotorFailureSystem::Configure(const Entity &_entity,
+				   const std::shared_ptr<const sdf::Element> &_sdf,
+				   EntityComponentManager &_ecm,
+				   EventManager &/*_eventMgr*/)
+{
+	this->_model = Model(_entity);
+	this->_model_entity = _entity;
+
+	if (!this->_model.Valid(_ecm)) {
+		gzerr << "[MotorFailureSystem] plugin should be attached to a model "
+		      << "entity. Failed to initialize." << std::endl;
+		return;
+	}
+
+	// Get model name to use as namespace
+	std::string model_name = this->_model.Name(_ecm);
+
+	// Get Gazebo Transport topic name for motor failure number subscription
+	if (_sdf->HasElement("MotorFailureTopic")) {
+		this->_gz_topic = _sdf->Get<std::string>("MotorFailureTopic");
+
+	} else {
+		// Use Gazebo model-scoped topic naming convention
+		this->_gz_topic = "/model/" + model_name + "/motor_failure/motor_number";
+	}
+
+	// Subscribe to Gazebo Transport topic
+	if (!this->_node.Subscribe(this->_gz_topic, &MotorFailureSystem::MotorFailureNumberCallback, this)) {
+		gzerr << "[MotorFailureSystem] Error subscribing to topic [" << this->_gz_topic << "]" << std::endl;
+		return;
+	}
+
+	gzmsg << "[MotorFailureSystem] Subscribed to Gazebo Transport topic: " << this->_gz_topic << std::endl;
+	gzmsg << "[MotorFailureSystem] Initialized for model: " << model_name << std::endl;
+}
+
+//////////////////////////////////////////////////
+void MotorFailureSystem::FindMotorJoints(EntityComponentManager &_ecm)
+{
+	if (this->_joints_found) {
+		return;
+	}
+
+	// Find all joints with "rotor_X_joint" pattern
+	this->_motor_joints.clear();
+
+	// Get all joints in the model
+	auto joints = this->_model.Joints(_ecm);
+
+	// Regular expression to match rotor joints (e.g., "rotor_0_joint", "rotor_1_joint")
+	std::regex motorPattern("rotor_(\\d+)_joint");
+	std::smatch match;
+
+	// Find rotor joints and sort by motor number
+	std::map<int, Entity> motorMap;
+
+	for (const auto &joint : joints) {
+		auto nameComp = _ecm.Component<components::Name>(joint);
+
+		if (nameComp) {
+			std::string jointName = nameComp->Data();
+
+			// Try to match the joint name against the pattern
+			if (std::regex_match(jointName, match, motorPattern)) {
+				// Extract motor number from the first capture group
+				try {
+					int motorNumber = std::stoi(match[1].str());
+					motorMap[motorNumber] = joint;
+					gzdbg << "[MotorFailureSystem] Found motor " << motorNumber
+					      << ": " << jointName << std::endl;
+
+				} catch (const std::exception &e) {
+					gzwarn << "[MotorFailureSystem] Failed to parse motor number from: "
+					       << jointName << std::endl;
+				}
+			}
+		}
+	}
+
+	// Convert map to vector for indexed access
+	for (const auto &pair : motorMap) {
+		// Ensure vector is large enough
+		if (pair.first >= static_cast<int>(this->_motor_joints.size())) {
+			this->_motor_joints.resize(pair.first + 1, kNullEntity);
+		}
+
+		this->_motor_joints[pair.first] = pair.second;
+	}
+
+	if (!this->_motor_joints.empty()) {
+		gzmsg << "[MotorFailureSystem] Found " << this->_motor_joints.size()
+		      << " motor joints" << std::endl;
+		this->_joints_found = true;
+
+	} else {
+		gzwarn << "[MotorFailureSystem] No motor joints found in model" << std::endl;
+	}
+}
+
+//////////////////////////////////////////////////
+void MotorFailureSystem::ApplyMotorFailure(EntityComponentManager &_ecm)
+{
+	int32_t current_failure;
+	{
+		std::lock_guard<std::mutex> lock(this->_motor_failure_mutex);
+		current_failure = this->_motor_failure_number;
+	}
+
+	// Check if failure status changed
+	if (current_failure != this->_prev_motor_failure_number) {
+		if (current_failure > 0) {
+			gzerr << "[MotorFailureSystem] Motor " << current_failure << " failed!" << std::endl;
+
+		} else if (current_failure == 0 && this->_prev_motor_failure_number > 0) {
+			gzerr << "[MotorFailureSystem] Motor " << this->_prev_motor_failure_number
+			      << " recovered!" << std::endl;
+		}
+
+		this->_prev_motor_failure_number = current_failure;
+	}
+
+	// Apply motor failure if active (1-indexed motor number, convert to 0-indexed)
+	if (current_failure > 0 && current_failure <= static_cast<int32_t>(this->_motor_joints.size())) {
+		int motorIdx = current_failure - 1;
+		Entity jointEntity = this->_motor_joints[motorIdx];
+
+		if (jointEntity != kNullEntity) {
+			// Force joint velocity command to 0
+			// This is done in PreUpdate to override MulticopterMotorModel's commands
+			auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(jointEntity);
+
+			if (jointVelCmd) {
+				*jointVelCmd = components::JointVelocityCmd({0.0});
+			}
+		}
+	}
+}
+
+//////////////////////////////////////////////////
+void MotorFailureSystem::PreUpdate(const UpdateInfo &_info,
+				   EntityComponentManager &_ecm)
+{
+	// Skip if paused
+	if (_info.paused) {
+		return;
+	}
+
+	// Find motor joints on first update
+	if (!this->_joints_found) {
+		this->FindMotorJoints(_ecm);
+
+	} else {
+		this->ApplyMotorFailure(const_cast<EntityComponentManager &>(_ecm));
+	}
+}
+
+//////////////////////////////////////////////////
+void MotorFailureSystem::MotorFailureNumberCallback(const gz::msgs::Int32 &_msg)
+{
+	std::lock_guard<std::mutex> lock(this->_motor_failure_mutex);
+	this->_motor_failure_number = _msg.data();
+	gzdbg << "[MotorFailureSystem] Received motor failure number: "
+	      << this->_motor_failure_number << std::endl;
+}
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MotorFailureSystem,
+	gz::sim::System,
+	MotorFailureSystem::ISystemConfigure,
+	MotorFailureSystem::ISystemPreUpdate
+)
+
+GZ_ADD_PLUGIN_ALIAS(MotorFailureSystem,
+		    "gz::sim::systems::MotorFailureSystem")

src/modules/simulation/gz_plugins/motor_failure/MotorFailureSystem.hpp

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_
+#define GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_
+
+#include <gz/sim/System.hh>
+#include <gz/sim/Model.hh>
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+#include <map>
+#include <regex>
+
+// Gazebo Transport includes
+#include <gz/transport/Node.hh>
+#include <gz/msgs/int32.pb.h>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE
+{
+namespace systems
+{
+
+/// \brief This system subscribes to a Gazebo Transport topic to receive motor failure
+/// commands and directly controls motor joints to simulate failures.
+/// This allows simulating motor failures in multirotor vehicles.
+class MotorFailureSystem:
+	public System,
+	public ISystemConfigure,
+	public ISystemPreUpdate
+{
+public:
+	// Documentation inherited
+	void Configure(const Entity &_entity,
+		       const std::shared_ptr<const sdf::Element> &_sdf,
+		       EntityComponentManager &_ecm,
+		       EventManager &_eventMgr) override;
+
+	// Documentation inherited
+	void PreUpdate(const gz::sim::UpdateInfo &_info,
+		       gz::sim::EntityComponentManager &_ecm) override;
+
+private:
+	/// \brief Callback for Gazebo Transport motor failure number subscription
+	void MotorFailureNumberCallback(const gz::msgs::Int32 &_msg);
+
+	/// \brief Find all motor joints in the model
+	void FindMotorJoints(gz::sim::EntityComponentManager &_ecm);
+
+	/// \brief Apply motor failure (set velocity to 0)
+	void ApplyMotorFailure(gz::sim::EntityComponentManager &_ecm);
+
+	/// \brief Gazebo Transport node for communication
+	gz::transport::Node _node;
+
+	/// \brief Model entity
+	gz::sim::Entity _model_entity;
+
+	/// \brief Model interface
+	gz::sim::Model _model;
+
+	/// \brief Vector of motor joint entities (indexed by motor number)
+	std::vector<gz::sim::Entity> _motor_joints;
+
+	/// \brief Current motor failure number (-1 or 0 means no failure, 1-indexed motor number)
+	int32_t _motor_failure_number{-1};
+
+	/// \brief Previous motor failure number to detect changes
+	int32_t _prev_motor_failure_number{-1};
+
+	/// \brief Gazebo Transport topic name for subscribing to motor failure commands
+	/// Defaults to /model/<model_name>/motor_failure/motor_number if not specified in SDF
+	std::string _gz_topic{"/motor_failure/motor_number"};
+
+	/// \brief Mutex to protect _motor_failure_number
+	std::mutex _motor_failure_mutex;
+
+	/// \brief Flag to indicate if motor joints have been found
+	bool _joints_found{false};
+};
+
+}  // namespace systems
+}  // namespace GZ_SIM_VERSION_NAMESPACE
+}  // namespace sim
+}  // namespace gz
+
+#endif  // GZ_SIM_SYSTEMS_MOTORFAILURESYSTEM_HPP_

src/modules/simulation/gz_plugins/motor_failure/README.md

@@ -0,0 +1,85 @@
+# Motor Failure System Plugin
+
+This Gazebo plugin enables motor failure simulation for multirotor vehicles in PX4 SITL.
+
+## Overview
+
+The Motor Failure System plugin subscribes to a Gazebo Transport topic to receive motor failure commands and directly controls motor joints to simulate failures. This allows simulating motor failures during flight testing.
+
+## Features
+
+- Gazebo Transport integration for receiving motor failure commands
+- Automatic detection of motor joints (rotor_0_joint, rotor_1_joint, etc.)
+- Direct joint velocity override in PreUpdate cycle
+- Thread-safe motor failure number handling
+- Configurable topic names via SDF
+
+## Configuration
+
+### SDF Parameters
+
+- `<MotorFailureTopic>` (optional): Gazebo Transport topic for receiving motor failure commands
+  - Default: `/model/<model_name>/motor_failure/motor_number`
+  - Follows Gazebo model-scoped topic naming convention
+  - If specified: Uses the exact topic name provided
+
+### Example SDF Usage
+
+**IMPORTANT**: The MotorFailureSystem plugin must be declared **AFTER** the `gz-sim-multicopter-motor-model-system` plugin in your SDF file. This ensures the motor failure override runs after the motor model sets its velocity commands.
+
+```xml
+<!-- First: Motor model plugin -->
+<plugin
+    filename="gz-sim-multicopter-motor-model-system"
+    name="gz::sim::systems::MulticopterMotorModel">
+    <!-- motor model configuration -->
+</plugin>
+
+<!-- Second: Motor failure plugin (must come after motor model) -->
+
+<!-- Minimal configuration (uses model name as namespace) -->
+<plugin
+    filename="MotorFailurePlugin"
+    name="gz::sim::systems::MotorFailureSystem">
+</plugin>
+
+<!-- Custom topic name -->
+<plugin
+    filename="MotorFailurePlugin"
+    name="gz::sim::systems::MotorFailureSystem">
+    <MotorFailureTopic>/custom/topic/name</MotorFailureTopic>
+</plugin>
+```
+
+## Usage
+
+### Publishing Motor Failure Commands
+
+To trigger a motor failure, publish a message to the Gazebo Transport topic.
+
+For a vehicle with namespace `x500_0`:
+
+```bash
+# Fail motor 1 (motors are 1-indexed: 1, 2, 3, 4, ...)
+gz topic -t /model/x500_0/motor_failure/motor_number -m gz.msgs.Int32 -p "data: 1"
+
+# Clear motor failure (restore normal operation)
+gz topic -t /model/x500_0/motor_failure/motor_number -m gz.msgs.Int32 -p "data: 0"
+```
+
+**Note**: Replace `x500_0` with your vehicle's model name.
+
+**Motor Numbering**:
+- Motors are **1-indexed**: 1, 2, 3, 4, etc.
+- `data: 0` clears the motor failure
+- `data: -1` also clears the motor failure
+
+### Monitoring Motor Failure Status
+
+You can monitor the motor failure messages in the Gazebo console output.
+
+
+## Notes
+
+- The plugin applies motor failure in the PreUpdate cycle by setting joint velocity to 0
+- **Plugin Declaration Order**: This plugin must be declared AFTER the MulticopterMotorModel plugin in the SDF file to ensure proper execution order